Merge pull request #65 from lge-ros2/main

Merge into humble

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.4.0</version>
+  <version>3.4.1</version>
   <description>node for actor plugin</description>
 
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

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.4.0</version>
+  <version>3.4.1</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>

cloisim_ros_bridge_zmq/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_bridge_zmq</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>bridge for cloisim(simulator) connection through ZMQ</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

cloisim_ros_bringup/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_bringup</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
 
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

cloisim_ros_bringup_param/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_bringup_param</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
 
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

cloisim_ros_camera/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_camera</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>virtual camera for cloisim</description>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

cloisim_ros_depthcamera/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_depthcamera</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>virtual depth camera for simulator</description>
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>

cloisim_ros_elevator_system/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_elevator_system</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>elevator system for simulation</description>
 
   <maintainer email="sungkyu.kang@lge.com">Sungkyu Kang</maintainer>

cloisim_ros_gps/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_gps</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>virtual gps for simulation</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

cloisim_ros_ground_truth/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_ground_truth</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>world plugin to retrieve ground truth</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

cloisim_ros_imu/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_imu</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>virtual imu for simulation</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

cloisim_ros_joint_control/CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(cloisim_ros_bringup_param REQUIRED)
 
 set(dependencies
   std_srvs
+  std_msgs
   sensor_msgs
   control_msgs
   cloisim_ros_base

cloisim_ros_joint_control/include/cloisim_ros_joint_control/joint_control.hpp

@@ -15,15 +15,13 @@
 #ifndef CLOISIM_ROS_JOINT_CONTROL__JOINT_CONTROL_HPP_
 #define CLOISIM_ROS_JOINT_CONTROL__JOINT_CONTROL_HPP_
 
-#include <cloisim_msgs/joint_cmd.pb.h>
-#include <cloisim_msgs/joint_state_v.pb.h>
-
 #include <map>
 #include <string>
 
 #include <cloisim_ros_base/base.hpp>
 #include <control_msgs/msg/joint_jog.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <std_srvs/srv/empty.hpp>
 
 namespace cloisim_ros
@@ -44,21 +42,24 @@ class JointControl : public Base
 
   void JointControlWrite(zmq::Bridge *const bridge_ptr, const std::string &buffer);
 
-  std::string MakeCommandMessage(
-      const std::string joint_name,
-      const bool use_displacement, const bool use_velocity,
-      const double joint_displacement = 0, const double joint_velocity = 0) const;
+  std::string MakeCommandMessage(control_msgs::msg::JointJog::ConstSharedPtr msg);
+
+  void GetRobotDescription(zmq::Bridge *const bridge_ptr);
 
  private:
   zmq::Bridge *info_bridge_ptr;
 
   std::map<std::string, std::string> target_transform_name;
 
+  std_msgs::msg::String msg_description_;
+
   // ROS2 JointControl publisher
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_joint_state_;
 
   // ROS2 Joint Jog subscriber
   rclcpp::Subscription<control_msgs::msg::JointJog>::SharedPtr sub_joint_job_;
+
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_robot_desc_;
 };
 }  // namespace cloisim_ros
 #endif  // CLOISIM_ROS_JOINT_CONTROL__JOINT_CONTROL_HPP_

cloisim_ros_joint_control/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_joint_control</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>joint_control package for simulator</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>
@@ -13,6 +13,7 @@
   <depend>cloisim_ros_bringup_param</depend>
   <depend>cloisim_ros_base</depend>
   <depend>std_srvs</depend>
+  <depend>std_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>control_msgs</depend>
 

cloisim_ros_joint_control/src/joint_control.cpp

@@ -14,9 +14,9 @@
  */
 
 #include "cloisim_ros_joint_control/joint_control.hpp"
-#include <cloisim_msgs/param.pb.h>
-#include <cloisim_msgs/transform_stamped.pb.h>
-#include <cloisim_msgs/twist.pb.h>
+#include <cloisim_msgs/joint_cmd_v.pb.h>
+#include <cloisim_msgs/joint_state_v.pb.h>
+
 #include <cloisim_ros_base/helper.h>
 #include <tf2/LinearMath/Matrix3x3.h>
 #include <tf2/LinearMath/Quaternion.h>
@@ -31,6 +31,9 @@ namespace cloisim_ros
 
 JointControl::JointControl(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_)
     : Base(node_name, namespace_, options_)
+    , pub_joint_state_(nullptr)
+    , sub_joint_job_(nullptr)
+    , pub_robot_desc_(nullptr)
 {
   Start();
 }
@@ -60,13 +63,38 @@ void JointControl::Initialize()
   DBG_SIM_INFO("hashKey: pub(%s) sub(%s) tf(%s)", hashKeyPub.c_str(), hashKeySub.c_str(), hashKeyTf.c_str());
 
   auto info_bridge_ptr = CreateBridge();
+  auto data_bridge_ptr = CreateBridge();
+  auto tf_bridge_ptr = CreateBridge();
+
+  auto callback_sub = [this, data_bridge_ptr](
+                          const control_msgs::msg::JointJog::SharedPtr msg) -> void
+  {
+    const auto msgBuf = MakeCommandMessage(msg);
+    SetBufferToSimulator(data_bridge_ptr, msgBuf);
+  };
+
+  {
+    // ROS2 Publisher
+    pub_joint_state_ = create_publisher<sensor_msgs::msg::JointState>(
+        "joint_states", rclcpp::SensorDataQoS());
+
+    // ROS2 Subscriber
+    sub_joint_job_ = create_subscription<control_msgs::msg::JointJog>(
+        "joint_command", rclcpp::SensorDataQoS(), callback_sub);
+
+    pub_robot_desc_ = create_publisher<std_msgs::msg::String>(
+        "robot_description", rclcpp::QoS(1).transient_local());
+  }
+
   if (info_bridge_ptr != nullptr)
   {
     info_bridge_ptr->Connect(zmq::Bridge::Mode::CLIENT, portInfo, hashKeyInfo);
+
     GetStaticTransforms(info_bridge_ptr);
+
+    GetRobotDescription(info_bridge_ptr);
   }
 
-  auto data_bridge_ptr = CreateBridge();
   if (data_bridge_ptr != nullptr)
   {
     data_bridge_ptr->Connect(zmq::Bridge::Mode::PUB, portRx, hashKeyPub);
@@ -75,74 +103,62 @@ void JointControl::Initialize()
                        bind(&JointControl::PublishData, this, std::placeholders::_1));
   }
 
-  auto tf_bridge_ptr = CreateBridge();
   if (tf_bridge_ptr != nullptr)
   {
     tf_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portTf, hashKeyTf);
     AddPublisherThread(tf_bridge_ptr,
                        bind(&Base::GenerateTF, this, std::placeholders::_1));
   }
 
-  auto callback_sub = [this, data_bridge_ptr](
-                          const control_msgs::msg::JointJog::SharedPtr msg) -> void
-  {
-    // const auto duration = msg->duration;
-    const auto use_displacement = (msg->joint_names.size() == msg->displacements.size());
-    const auto use_velocity = (msg->joint_names.size() == msg->velocities.size());
-
-    for (size_t i = 0; i < msg->joint_names.size(); i++)
-    {
-      const auto joint_name = msg->joint_names[i];
-      const auto displacement = (use_displacement) ? msg->displacements[i] : 0;
-      const auto velocity = (use_velocity) ? msg->velocities[i] : 0;
-
-      DBG_SIM_INFO("%s %f %f", joint_name.c_str(), displacement, velocity);
-      const auto msgBuf = MakeCommandMessage(joint_name, use_displacement, use_velocity, displacement, velocity);
-      SetBufferToSimulator(data_bridge_ptr, msgBuf);
-      rclcpp::sleep_for(500us);
-    }
-  };
-
-  // ROS2 Publisher
-  pub_joint_state_ = create_publisher<sensor_msgs::msg::JointState>(
-      "joint_states", rclcpp::SensorDataQoS());
-
-  // ROS2 Subscriber
-  sub_joint_job_ = create_subscription<control_msgs::msg::JointJog>(
-      "joint_command", rclcpp::SensorDataQoS(), callback_sub);
+  if (pub_robot_desc_ != nullptr)
+    pub_robot_desc_->publish(msg_description_);
 }
 
 string JointControl::MakeCommandMessage(
-    const string joint_name,
-    const bool use_displacement,
-    const bool use_velocity,
-    const double joint_displacement,
-    const double joint_velocity) const
+    control_msgs::msg::JointJog::ConstSharedPtr msg)
 {
-  msgs::JointCmd jointCmd;
+  msgs::JointCmd_V pb_joint_cmds;
 
-  jointCmd.set_name(joint_name);
+  // const auto duration = msg->duration;
+  const auto use_displacement = (msg->joint_names.size() == msg->displacements.size());
+  const auto use_velocity = (msg->joint_names.size() == msg->velocities.size());
 
-  if (use_displacement)
+  for (size_t i = 0; i < msg->joint_names.size(); i++)
   {
-    auto position = jointCmd.mutable_position();
-    position->set_target(joint_displacement);
-  }
+    auto joint_cmd = pb_joint_cmds.add_jointcmd();
+    const auto joint_name = msg->joint_names[i];
+    const auto joint_displacement = (use_displacement) ? msg->displacements[i] : 0;
+    const auto joint_velocity = (use_velocity) ? msg->velocities[i] : 0;
 
-  if (use_velocity)
-  {
-    auto velocity = jointCmd.mutable_velocity();
-    velocity->set_target(joint_velocity);
+    // DBG_SIM_INFO("%s %f %f", joint_name.c_str(), displacement, velocity);
+
+    joint_cmd->set_name(joint_name);
+
+    if (use_displacement)
+    {
+      auto position = joint_cmd->mutable_position();
+      position->set_target(joint_displacement);
+    }
+
+    if (use_velocity)
+    {
+      auto velocity = joint_cmd->mutable_velocity();
+      velocity->set_target(joint_velocity);
+    }
   }
 
+  auto time = pb_joint_cmds.mutable_time();
+  time->set_sec(GetTime().seconds());
+  time->set_nsec(GetTime().nanoseconds());
+
   string message;
-  jointCmd.SerializeToString(&message);
+  pb_joint_cmds.SerializeToString(&message);
   return message;
 }
 
 void JointControl::PublishData(const string &buffer)
 {
-  cloisim::msgs::JointState_V pb_joint_states;
+  msgs::JointState_V pb_joint_states;
   if (!pb_joint_states.ParseFromString(buffer))
   {
     DBG_SIM_ERR("Parsing error, size(%d)", buffer.length());
@@ -169,7 +185,28 @@ void JointControl::PublishData(const string &buffer)
   }
 
   // publish data
-  pub_joint_state_->publish(msg_jointstate);
+  if (pub_joint_state_ != nullptr)
+    pub_joint_state_->publish(msg_jointstate);
+}
+
+void JointControl::GetRobotDescription(zmq::Bridge *const bridge_ptr)
+{
+  if (bridge_ptr == nullptr)
+  {
+    return;
+  }
+
+  const auto reply = RequestReplyMessage(bridge_ptr, "robot_description");
+
+  if (reply.IsInitialized() &&
+      (reply.name().compare("description") == 0))
+  {
+    if (reply.value().type() == msgs::Any_ValueType_STRING)
+    {
+      const auto description = reply.value().string_value();
+      msg_description_.data = description;
+    }
+  }
 }
 
 }  // namespace cloisim_ros

cloisim_ros_lidar/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_lidar</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>virtual lidar for simulation</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

cloisim_ros_micom/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_micom</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>micom package for simulator</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>
   <author email="hyunseok7.yang@lge.com">Hyunseok Yang</author>

cloisim_ros_msgs/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_msgs</name>
-  <version>3.4.0</version>
+  <version>3.4.1</version>
   <description>interfaces package for cloisim_ros</description>
   <maintainer email="hyunseok7.yang@lge.com">Hyunseok Yang</maintainer>