diff --git a/cloisim_ros_actor/package.xml b/cloisim_ros_actor/package.xml index ad61ee73..baf6a735 100644 --- a/cloisim_ros_actor/package.xml +++ b/cloisim_ros_actor/package.xml @@ -2,7 +2,7 @@ cloisim_ros_actor - 3.4.0 + 3.4.1 node for actor plugin Hyunseok Yang diff --git a/cloisim_ros_base/package.xml b/cloisim_ros_base/package.xml index 155e65b5..4a7b1981 100644 --- a/cloisim_ros_base/package.xml +++ b/cloisim_ros_base/package.xml @@ -2,7 +2,7 @@ cloisim_ros_base - 3.4.0 + 3.4.1 CLOiSim-ROS base class for other CLOiSim-ROS Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_bridge_zmq/package.xml b/cloisim_ros_bridge_zmq/package.xml index f93f8067..706fee8e 100644 --- a/cloisim_ros_bridge_zmq/package.xml +++ b/cloisim_ros_bridge_zmq/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bridge_zmq - 3.4.0 + 3.4.1 bridge for cloisim(simulator) connection through ZMQ Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_bringup/package.xml b/cloisim_ros_bringup/package.xml index 1a0dd202..e7a2a781 100644 --- a/cloisim_ros_bringup/package.xml +++ b/cloisim_ros_bringup/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bringup - 3.4.0 + 3.4.1 Bringup scripts and configurations for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_bringup_param/package.xml b/cloisim_ros_bringup_param/package.xml index eb8403c9..fb8857f1 100644 --- a/cloisim_ros_bringup_param/package.xml +++ b/cloisim_ros_bringup_param/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bringup_param - 3.4.0 + 3.4.1 Bringup scripts and configurations for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_camera/package.xml b/cloisim_ros_camera/package.xml index 10c56e30..9c8c2d7b 100644 --- a/cloisim_ros_camera/package.xml +++ b/cloisim_ros_camera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_camera - 3.4.0 + 3.4.1 virtual camera for cloisim Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_depthcamera/package.xml b/cloisim_ros_depthcamera/package.xml index d4fdc004..78ee52cb 100644 --- a/cloisim_ros_depthcamera/package.xml +++ b/cloisim_ros_depthcamera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_depthcamera - 3.4.0 + 3.4.1 virtual depth camera for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_elevator_system/package.xml b/cloisim_ros_elevator_system/package.xml index 65b4f546..4bc80ef4 100644 --- a/cloisim_ros_elevator_system/package.xml +++ b/cloisim_ros_elevator_system/package.xml @@ -2,7 +2,7 @@ cloisim_ros_elevator_system - 3.4.0 + 3.4.1 elevator system for simulation Sungkyu Kang diff --git a/cloisim_ros_gps/package.xml b/cloisim_ros_gps/package.xml index 881f290a..0d1ab309 100644 --- a/cloisim_ros_gps/package.xml +++ b/cloisim_ros_gps/package.xml @@ -2,7 +2,7 @@ cloisim_ros_gps - 3.4.0 + 3.4.1 virtual gps for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_ground_truth/package.xml b/cloisim_ros_ground_truth/package.xml index af48066d..faaac4ab 100644 --- a/cloisim_ros_ground_truth/package.xml +++ b/cloisim_ros_ground_truth/package.xml @@ -2,7 +2,7 @@ cloisim_ros_ground_truth - 3.4.0 + 3.4.1 world plugin to retrieve ground truth Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_imu/package.xml b/cloisim_ros_imu/package.xml index 6447e70e..f3cb476e 100644 --- a/cloisim_ros_imu/package.xml +++ b/cloisim_ros_imu/package.xml @@ -2,7 +2,7 @@ cloisim_ros_imu - 3.4.0 + 3.4.1 virtual imu for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_joint_control/CMakeLists.txt b/cloisim_ros_joint_control/CMakeLists.txt index df7b69cd..53f1ed6a 100644 --- a/cloisim_ros_joint_control/CMakeLists.txt +++ b/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 diff --git a/cloisim_ros_joint_control/include/cloisim_ros_joint_control/joint_control.hpp b/cloisim_ros_joint_control/include/cloisim_ros_joint_control/joint_control.hpp index 2f6ef54c..0d800bd4 100644 --- a/cloisim_ros_joint_control/include/cloisim_ros_joint_control/joint_control.hpp +++ b/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 -#include - #include #include #include #include #include +#include #include 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 target_transform_name; + std_msgs::msg::String msg_description_; + // ROS2 JointControl publisher rclcpp::Publisher::SharedPtr pub_joint_state_; // ROS2 Joint Jog subscriber rclcpp::Subscription::SharedPtr sub_joint_job_; + + rclcpp::Publisher::SharedPtr pub_robot_desc_; }; } // namespace cloisim_ros #endif // CLOISIM_ROS_JOINT_CONTROL__JOINT_CONTROL_HPP_ diff --git a/cloisim_ros_joint_control/package.xml b/cloisim_ros_joint_control/package.xml index 261192a9..0d417d08 100644 --- a/cloisim_ros_joint_control/package.xml +++ b/cloisim_ros_joint_control/package.xml @@ -2,7 +2,7 @@ cloisim_ros_joint_control - 3.4.0 + 3.4.1 joint_control package for simulator Hyunseok Yang Hyunseok Yang @@ -13,6 +13,7 @@ cloisim_ros_bringup_param cloisim_ros_base std_srvs + std_msgs sensor_msgs control_msgs diff --git a/cloisim_ros_joint_control/src/joint_control.cpp b/cloisim_ros_joint_control/src/joint_control.cpp index 6f59409b..10420685 100644 --- a/cloisim_ros_joint_control/src/joint_control.cpp +++ b/cloisim_ros_joint_control/src/joint_control.cpp @@ -14,9 +14,9 @@ */ #include "cloisim_ros_joint_control/joint_control.hpp" -#include -#include -#include +#include +#include + #include #include #include @@ -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( + "joint_states", rclcpp::SensorDataQoS()); + + // ROS2 Subscriber + sub_joint_job_ = create_subscription( + "joint_command", rclcpp::SensorDataQoS(), callback_sub); + + pub_robot_desc_ = create_publisher( + "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,7 +103,6 @@ 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); @@ -83,66 +110,55 @@ void JointControl::Initialize() 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( - "joint_states", rclcpp::SensorDataQoS()); - - // ROS2 Subscriber - sub_joint_job_ = create_subscription( - "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 diff --git a/cloisim_ros_lidar/package.xml b/cloisim_ros_lidar/package.xml index 90b735b0..34fad140 100644 --- a/cloisim_ros_lidar/package.xml +++ b/cloisim_ros_lidar/package.xml @@ -2,7 +2,7 @@ cloisim_ros_lidar - 3.4.0 + 3.4.1 virtual lidar for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_micom/package.xml b/cloisim_ros_micom/package.xml index d6c8acf0..ee7881e1 100644 --- a/cloisim_ros_micom/package.xml +++ b/cloisim_ros_micom/package.xml @@ -2,7 +2,7 @@ cloisim_ros_micom - 3.4.0 + 3.4.1 micom package for simulator Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_msgs/package.xml b/cloisim_ros_msgs/package.xml index 4c4bfcd2..73a38d59 100644 --- a/cloisim_ros_msgs/package.xml +++ b/cloisim_ros_msgs/package.xml @@ -2,7 +2,7 @@ cloisim_ros_msgs - 3.4.0 + 3.4.1 interfaces package for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_multicamera/package.xml b/cloisim_ros_multicamera/package.xml index cb3356e7..cbcb4dde 100644 --- a/cloisim_ros_multicamera/package.xml +++ b/cloisim_ros_multicamera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_multicamera - 3.4.0 + 3.4.1 virtual multi-camera for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_protobuf_msgs/CMakeLists.txt b/cloisim_ros_protobuf_msgs/CMakeLists.txt index d65b5cca..69f7f17e 100644 --- a/cloisim_ros_protobuf_msgs/CMakeLists.txt +++ b/cloisim_ros_protobuf_msgs/CMakeLists.txt @@ -25,7 +25,7 @@ list(APPEND PROTOBUF_DEFINITION_FILES "micom;battery;pointcloud;gps") list(APPEND PROTOBUF_DEFINITION_FILES "world_stats;log_playback_stats") list(APPEND PROTOBUF_DEFINITION_FILES "perception;perception_v") list(APPEND PROTOBUF_DEFINITION_FILES "request;response") -list(APPEND PROTOBUF_DEFINITION_FILES "joint_state;joint_state_v;joint_cmd;pid") +list(APPEND PROTOBUF_DEFINITION_FILES "joint_state;joint_state_v;joint_cmd;joint_cmd_v;pid") list(APPEND PROTOBUF_DEFINITION_FILES "transform_stamped") set(PROTOBUF_IMPORT_DIRS) diff --git a/cloisim_ros_protobuf_msgs/msgs/joint_cmd_v.proto b/cloisim_ros_protobuf_msgs/msgs/joint_cmd_v.proto new file mode 100644 index 00000000..a5a94e1a --- /dev/null +++ b/cloisim_ros_protobuf_msgs/msgs/joint_cmd_v.proto @@ -0,0 +1,15 @@ +syntax = "proto2"; +package cloisim.msgs; + +/// \ingroup cloisim_msgs +/// \interface JointCmd_V +/// \brief Message for joint command, used by cloisim_ros_joint_control + +import "time.proto"; +import "joint_cmd.proto"; + +message JointCmd_V +{ + required Time time = 1; + repeated JointCmd JointCmd = 2; +} diff --git a/cloisim_ros_protobuf_msgs/package.xml b/cloisim_ros_protobuf_msgs/package.xml index 0214dd1f..1dde93e3 100644 --- a/cloisim_ros_protobuf_msgs/package.xml +++ b/cloisim_ros_protobuf_msgs/package.xml @@ -2,7 +2,7 @@ cloisim_ros_protobuf_msgs - 3.4.0 + 3.4.1 CLOiSim-ROS interafces for communication between simulator and CLOiSim-ROS Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_realsense/package.xml b/cloisim_ros_realsense/package.xml index b7945d80..be5af598 100644 --- a/cloisim_ros_realsense/package.xml +++ b/cloisim_ros_realsense/package.xml @@ -2,7 +2,7 @@ cloisim_ros_realsense - 3.4.0 + 3.4.1 virtual realsense for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_sonar/package.xml b/cloisim_ros_sonar/package.xml index 9536e743..bb1ad3c6 100644 --- a/cloisim_ros_sonar/package.xml +++ b/cloisim_ros_sonar/package.xml @@ -2,7 +2,7 @@ cloisim_ros_sonar - 3.4.0 + 3.4.1 virtual sonar for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_websocket_service/package.xml b/cloisim_ros_websocket_service/package.xml index 4223a17c..b74c899f 100644 --- a/cloisim_ros_websocket_service/package.xml +++ b/cloisim_ros_websocket_service/package.xml @@ -2,7 +2,7 @@ cloisim_ros_websocket_service - 3.4.0 + 3.4.1 websocket service for cloisim(simulator) connection port control Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_world/package.xml b/cloisim_ros_world/package.xml index 814c3e2d..af2501ce 100644 --- a/cloisim_ros_world/package.xml +++ b/cloisim_ros_world/package.xml @@ -2,7 +2,7 @@ cloisim_ros_world - 3.4.0 + 3.4.1 Utilities to interface with Unity through ROS. Hyunseok Yang Hyunseok Yang