Merge pull request #63 from lge-ros2/main

Merge into humble
lge-ros2 · Sep 27, 2023 · 7115a4a · 7115a4a
2 parents 88bf172 + 93b1aec
commit 7115a4a
Show file tree

Hide file tree

Showing 70 changed files with 1,020 additions and 697 deletions.
diff --git a/cloisim_ros_actor/include/cloisim_ros_actor/actor.hpp b/cloisim_ros_actor/include/cloisim_ros_actor/actor.hpp
@@ -13,38 +13,45 @@
  *      SPDX-License-Identifier: MIT
  */
 
-#ifndef _CLOISIM_ROS_ACTOR_HPP_
-#define _CLOISIM_ROS_ACTOR_HPP_
+#ifndef CLOISIM_ROS_ACTOR__ACTOR_HPP_
+#define CLOISIM_ROS_ACTOR__ACTOR_HPP_
 
-#include <cloisim_ros_base/base.hpp>
 #include <cloisim_msgs/param.pb.h>
+
+#include <memory>
+#include <string>
+
+#include <cloisim_ros_base/base.hpp>
 #include <cloisim_ros_msgs/srv/move_actor.hpp>
 
 namespace cloisim_ros
 {
-  class Actor : public Base
-  {
-  public:
-    explicit Actor(const rclcpp::NodeOptions &options_, const std::string node_name);
-    explicit Actor();
-    virtual ~Actor();
-
-  private:
-    void Initialize() override;
-    void Deinitialize() override { };
-
-  private:
-    zmq::Bridge *control_bridge_ptr;
-
-    rclcpp::Service<cloisim_ros_msgs::srv::MoveActor>::SharedPtr srvCallMoveActor_;
-
-  private:
-    void CallMoveActor(const std::shared_ptr<rmw_request_id_t> /*request_header*/,
-                           const std::shared_ptr<cloisim_ros_msgs::srv::MoveActor::Request> request,
-                           const std::shared_ptr<cloisim_ros_msgs::srv::MoveActor::Response> response);
-
-    cloisim::msgs::Param CreateMoveRequest(const std::string target_name, const geometry_msgs::msg::Vector3 point);
-    bool GetResultFromResponse(const cloisim::msgs::Param &response_msg);
-  };
-}
-#endif
+class Actor : public Base
+{
+ public:
+  explicit Actor(const rclcpp::NodeOptions &options_, const std::string node_name);
+  Actor();
+  virtual ~Actor();
+
+ private:
+  void Initialize() override;
+  void Deinitialize() override{};
+
+ private:
+  zmq::Bridge *control_bridge_ptr;
+
+  rclcpp::Service<cloisim_ros_msgs::srv::MoveActor>::SharedPtr srvCallMoveActor_;
+
+ private:
+  void CallMoveActor(const std::shared_ptr<rmw_request_id_t> /*request_header*/,
+                     const std::shared_ptr<cloisim_ros_msgs::srv::MoveActor::Request> request,
+                     const std::shared_ptr<cloisim_ros_msgs::srv::MoveActor::Response> response);
+
+  cloisim::msgs::Param CreateMoveRequest(
+      const std::string target_name,
+      const geometry_msgs::msg::Vector3 point);
+
+  bool GetResultFromResponse(const cloisim::msgs::Param &response_msg);
+};
+}  // namespace cloisim_ros
+#endif  // CLOISIM_ROS_ACTOR__ACTOR_HPP_
diff --git a/cloisim_ros_actor/package.xml b/cloisim_ros_actor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_actor</name>
-  <version>3.3.1</version>
+  <version>3.4.0</version>
   <description>node for actor plugin</description>
 
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

diff --git a/cloisim_ros_actor/src/actor.cpp b/cloisim_ros_actor/src/actor.cpp
@@ -16,9 +16,11 @@
 #include "cloisim_ros_actor/actor.hpp"
 
 using namespace std;
+using namespace std::placeholders;
 using namespace cloisim;
-using namespace cloisim_ros;
-using namespace placeholders;
+
+namespace cloisim_ros
+{
 
 Actor::Actor(const rclcpp::NodeOptions &options_, const string node_name)
     : Base(node_name, options_)
@@ -51,7 +53,8 @@ void Actor::Initialize()
   {
     control_bridge_ptr->Connect(zmq::Bridge::Mode::CLIENT, portControl, hashKeySrv);
 
-    srvCallMoveActor_ = this->create_service<cloisim_ros_msgs::srv::MoveActor>(nodeName + "/move_actor", bind(&Actor::CallMoveActor, this, _1, _2, _3));
+    srvCallMoveActor_ = this->create_service<cloisim_ros_msgs::srv::MoveActor>(
+        nodeName + "/move_actor", bind(&Actor::CallMoveActor, this, _1, _2, _3));
   }
 }
 
@@ -77,7 +80,9 @@ bool Actor::GetResultFromResponse(const msgs::Param &response_msg)
   return response_msg.value().bool_value();
 }
 
-cloisim::msgs::Param Actor::CreateMoveRequest(const string target_name, const geometry_msgs::msg::Vector3 point)
+msgs::Param Actor::CreateMoveRequest(
+    const string target_name,
+    const geometry_msgs::msg::Vector3 point)
 {
   msgs::Param request_msg;
   request_msg.set_name(target_name);
@@ -91,4 +96,6 @@ cloisim::msgs::Param Actor::CreateMoveRequest(const string target_name, const ge
   vector3d_value_ptr->set_z(point.z);
 
   return request_msg;
-}
+}
+
+}  // namespace cloisim_ros
diff --git a/cloisim_ros_actor/src/main.cpp b/cloisim_ros_actor/src/main.cpp
@@ -54,4 +54,4 @@ int main(int argc, char *argv[])
   rclcpp::shutdown();
 
   return 0;
-}
+}
diff --git a/cloisim_ros_base/include/cloisim_ros_base/base.hpp b/cloisim_ros_base/include/cloisim_ros_base/base.hpp
@@ -13,40 +13,61 @@
  *      SPDX-License-Identifier: MIT
  */
 
-#ifndef _CLOISIM_ROS_BASE_HPP_
-#define _CLOISIM_ROS_BASE_HPP_
+#ifndef CLOISIM_ROS_BASE__BASE_HPP_
+#define CLOISIM_ROS_BASE__BASE_HPP_
 
-#include <cloisim_ros_bridge_zmq/bridge.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <vector>
 #include <cloisim_msgs/param.pb.h>
 #include <cloisim_msgs/pose.pb.h>
 #include <cloisim_msgs/time.pb.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_ros/transform_broadcaster.h>
 
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <cloisim_ros_bridge_zmq/bridge.hpp>
+#include <rclcpp/rclcpp.hpp>
+
 namespace cloisim_ros
 {
-static rclcpp::Time Convert(const int32_t seconds, const uint32_t nanoseconds) { return rclcpp::Time(seconds, nanoseconds); }
-static rclcpp::Time Convert(const cloisim::msgs::Time& time) { return Convert(time.sec(), time.nsec()); }
+
+static rclcpp::Time Convert(const int32_t seconds, const uint32_t nanoseconds)
+{
+  return rclcpp::Time(seconds, nanoseconds);
+}
+
+static rclcpp::Time Convert(const cloisim::msgs::Time& time)
+{
+  return Convert(time.sec(), time.nsec());
+}
 
 class Base : public rclcpp::Node
 {
  public:
   explicit Base(const std::string node_name);
   explicit Base(const std::string node_name, const rclcpp::NodeOptions& options);
   explicit Base(const std::string node_name, const std::string namespace_);
-  explicit Base(const std::string node_name, const std::string namespace_, const rclcpp::NodeOptions& options);
+  explicit Base(const std::string node_name, const std::string namespace_,
+                const rclcpp::NodeOptions& options);
   ~Base();
 
  protected:
   virtual void Initialize() = 0;
   virtual void Deinitialize() = 0;
 
  protected:
-  void SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const std::string child_frame_id, const std::string header_frame_id = "base_link");
-  void SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const cloisim::msgs::Pose transform, const std::string child_frame_id, const std::string header_frame_id = "base_link");
-  void SetStaticTf2(const cloisim::msgs::Pose transform, const std::string parent_header_frame_id = "base_link");
+  void SetTf2(geometry_msgs::msg::TransformStamped& target_msg,
+              const std::string child_frame_id,
+              const std::string header_frame_id = "base_link");
+
+  void SetTf2(geometry_msgs::msg::TransformStamped& target_msg,
+              const cloisim::msgs::Pose transform,
+              const std::string child_frame_id,
+              const std::string header_frame_id = "base_link");
+
+  void SetStaticTf2(const cloisim::msgs::Pose transform,
+                    const std::string parent_header_frame_id = "base_link");
 
   void Start(const bool enable_tf_publish = true);
   void Stop();
@@ -62,7 +83,8 @@ class Base : public rclcpp::Node
 
   void CloseBridges();
 
-  void AddPublisherThread(zmq::Bridge* const bridge_ptr, std::function<void(const std::string&)> thread_func);
+  void AddPublisherThread(zmq::Bridge* const bridge_ptr,
+                          std::function<void(const std::string&)> thread_func);
 
   std::string GetModelName();
   std::string GetRobotName();
@@ -73,20 +95,30 @@ class Base : public rclcpp::Node
   void PublishTF();
   void PublishTF(const geometry_msgs::msg::TransformStamped& tf);
 
-  cloisim::msgs::Pose GetObjectTransform(zmq::Bridge* const bridge_ptr, const std::string target_name = "");
+  void GetStaticTransforms(zmq::Bridge* const bridge_ptr);
 
-  cloisim::msgs::Pose GetObjectTransform(zmq::Bridge* const bridge_ptr, const std::string target_name, std::string& parent_frame_id);
+  cloisim::msgs::Pose GetObjectTransform(zmq::Bridge* const bridge_ptr,
+                                         const std::string target_name = "");
+
+  cloisim::msgs::Pose GetObjectTransform(zmq::Bridge* const bridge_ptr,
+                                         const std::string target_name,
+                                         std::string& parent_frame_id);
 
   void GetRos2Parameter(zmq::Bridge* const bridge_ptr);
 
-  static bool GetBufferFromSimulator(zmq::Bridge* const bridge_ptr, void** ppBbuffer, int& bufferLength, const bool isNonBlockingMode = false);
+  static bool GetBufferFromSimulator(zmq::Bridge* const bridge_ptr,
+                                     void** ppBbuffer, int& bufferLength,
+                                     const bool isNonBlockingMode = false);
 
   static bool SetBufferToSimulator(zmq::Bridge* const bridge_ptr, const std::string& buffer);
 
   std::string GetFrameId(const std::string default_frame_id = "base_link");
 
-  static cloisim::msgs::Param RequestReplyMessage(zmq::Bridge* const bridge_ptr, const std::string request_message, const std::string request_value = "");
-  static cloisim::msgs::Param RequestReplyMessage(zmq::Bridge* const bridge_ptr, const cloisim::msgs::Param request_message);
+  static cloisim::msgs::Param RequestReplyMessage(zmq::Bridge* const bridge_ptr,
+                                                  const std::string request_message,
+                                                  const std::string request_value = "");
+  static cloisim::msgs::Param RequestReplyMessage(zmq::Bridge* const bridge_ptr,
+                                                  const cloisim::msgs::Param request_message);
   static cloisim::msgs::Pose IdentityPose();
 
   void SetTime(const cloisim::msgs::Time& time);
@@ -138,7 +170,8 @@ inline void Base::PublishTF(const geometry_msgs::msg::TransformStamped& tf)
   m_tf_broadcaster->sendTransform(tf);
 }
 
-inline cloisim::msgs::Pose Base::GetObjectTransform(zmq::Bridge* const bridge_ptr, const std::string target_name)
+inline cloisim::msgs::Pose Base::GetObjectTransform(zmq::Bridge* const bridge_ptr,
+                                                    const std::string target_name)
 {
   std::string empty_arg("");
   return GetObjectTransform(bridge_ptr, target_name, empty_arg);
@@ -174,6 +207,5 @@ inline void Base::SetTime(const int32_t seconds, const uint32_t nanoseconds)
 {
   m_sim_time = Convert(seconds, nanoseconds);
 }
-
 }  // namespace cloisim_ros
-#endif
+#endif  // CLOISIM_ROS_BASE__BASE_HPP_
diff --git a/cloisim_ros_base/include/cloisim_ros_base/camera_helper.h b/cloisim_ros_base/include/cloisim_ros_base/camera_helper.h
@@ -13,14 +13,18 @@
  *      SPDX-License-Identifier: MIT
  */
 
-#ifndef _CLOISIM_ROS_CAMERA_HELPER_H_
-#define _CLOISIM_ROS_CAMERA_HELPER_H_
+#ifndef CLOISIM_ROS_BASE__CAMERA_HELPER_H_
+#define CLOISIM_ROS_BASE__CAMERA_HELPER_H_
+
+#include <cloisim_msgs/camerasensor.pb.h>
+#include <cloisim_msgs/param.pb.h>
+
+#include <memory>
+#include <string>
 
 #include <camera_info_manager/camera_info_manager.hpp>
 #include <cloisim_ros_bridge_zmq/bridge.hpp>
 #include <sensor_msgs/image_encodings.hpp>
-#include <cloisim_msgs/camerasensor.pb.h>
-#include <cloisim_msgs/param.pb.h>
 
 static std::string GetImageEncondingType(const uint32_t pixel_format)
 {
@@ -167,10 +171,11 @@ static cloisim::msgs::CameraSensor GetCameraSensorMessage(
   else
   {
     if (cameraSensorInfo.ParseFromString(reply) == false)
-      DBG_SIM_ERR("Failed to Parsing Proto buffer buffer_ptr(%p) length(%ld)", reply.data(), reply.size());
+      DBG_SIM_ERR("Failed to Parsing Proto buffer buffer_ptr(%p) length(%ld)",
+                  reply.data(), reply.size());
   }
 
   return cameraSensorInfo;
 }
 
-#endif
+#endif  // CLOISIM_ROS_BASE__CAMERA_HELPER_H_
diff --git a/cloisim_ros_base/include/cloisim_ros_base/helper.h b/cloisim_ros_base/include/cloisim_ros_base/helper.h
@@ -13,58 +13,64 @@
  *      SPDX-License-Identifier: MIT
  */
 
-#ifndef _CLOISIM_ROS_HELPER_H_
-#define _CLOISIM_ROS_HELPER_H_
+#ifndef CLOISIM_ROS_BASE__HELPER_H_
+#define CLOISIM_ROS_BASE__HELPER_H_
+
+#include <tf2/LinearMath/Quaternion.h>
 
-#include <geometry_msgs/msg/vector3.hpp>
-#include <geometry_msgs/msg/point32.hpp>
 #include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/point32.hpp>
 #include <geometry_msgs/msg/quaternion.hpp>
-#include <tf2/LinearMath/Quaternion.h>
+#include <geometry_msgs/msg/vector3.hpp>
 
-static void SetVector3MessageToGeometry(const cloisim::msgs::Vector3d &src, geometry_msgs::msg::Vector3 &dst)
+static void SetVector3MessageToGeometry(
+    const cloisim::msgs::Vector3d &src, geometry_msgs::msg::Vector3 &dst)
 {
   dst.x = src.x();
   dst.y = src.y();
   dst.z = src.z();
 }
 
-static void SetVector3MessageToGeometry(const cloisim::msgs::Vector3d &src, geometry_msgs::msg::Point32 &dst)
+static void SetVector3MessageToGeometry(
+    const cloisim::msgs::Vector3d &src, geometry_msgs::msg::Point32 &dst)
 {
   dst.x = src.x();
   dst.y = src.y();
   dst.z = src.z();
 }
 
-static void SetVector3MessageToGeometry(const cloisim::msgs::Vector3d &src, geometry_msgs::msg::Point &dst)
+static void SetVector3MessageToGeometry(
+    const cloisim::msgs::Vector3d &src, geometry_msgs::msg::Point &dst)
 {
   dst.x = src.x();
   dst.y = src.y();
   dst.z = src.z();
 }
 
-static void SetQuaternionMessageToGeometry(const cloisim::msgs::Quaternion &src, geometry_msgs::msg::Quaternion &dst)
+static void SetQuaternionMessageToGeometry(
+    const cloisim::msgs::Quaternion &src, geometry_msgs::msg::Quaternion &dst)
 {
   dst.x = src.x();
   dst.y = src.y();
   dst.z = src.z();
   dst.w = src.w();
 }
 
-static void SetTf2QuaternionToGeometry(const tf2::Quaternion &src, geometry_msgs::msg::Quaternion &dst)
+static void SetTf2QuaternionToGeometry(
+    const tf2::Quaternion &src, geometry_msgs::msg::Quaternion &dst)
 {
   dst.x = src.x();
   dst.y = src.y();
   dst.z = src.z();
   dst.w = src.w();
 }
 
-
-static void SetPointToGeometry(const geometry_msgs::msg::Point &src, geometry_msgs::msg::Vector3 &dst)
+static void SetPointToGeometry(
+    const geometry_msgs::msg::Point &src, geometry_msgs::msg::Vector3 &dst)
 {
   dst.x = src.x;
   dst.y = src.y;
   dst.z = src.z;
 }
 
-#endif
+#endif  // CLOISIM_ROS_BASE__HELPER_H_
diff --git a/cloisim_ros_base/package.xml b/cloisim_ros_base/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_base</name>
-  <version>3.3.1</version>
+  <version>3.4.0</version>
   <description>CLOiSim-ROS base class for other CLOiSim-ROS</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>
-Original file line number
+Diff line change
@@ Expand Up / @@ -54,4 +54,4 @@ int main(int argc, char *argv[]) @@
       rclcpp::shutdown();
       return 0;
-    }
+    }