From a7b61b699aef4ddf3fd5b5d2684cd59d143b37da Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 14 Aug 2023 18:15:47 +0900 Subject: [PATCH 1/9] cloisim_ros_realsense - determine topic name only whether module name is "depth" or not --- cloisim_ros_realsense/src/realsense.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cloisim_ros_realsense/src/realsense.cpp b/cloisim_ros_realsense/src/realsense.cpp index 3b157c8a..408279c9 100644 --- a/cloisim_ros_realsense/src/realsense.cpp +++ b/cloisim_ros_realsense/src/realsense.cpp @@ -119,7 +119,7 @@ void RealSense::InitializeCam(const string module_name, zmq::Bridge* const info_ camera_info_managers_[data_ptr] = camInfoManager; } - const auto topic_name = (module_name.find("depth") == string::npos) ? "image_raw" : "image_rect_raw"; + const auto topic_name = (module_name == "depth") ? "image_rect_raw" : "image_raw"; image_transport::ImageTransport it(GetNode()); From 2d1d3e1c7e313a9adc059bbc88ad832f4e675b52 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 14 Aug 2023 18:21:34 +0900 Subject: [PATCH 2/9] Add time sleep every node is created in bringup -> 100ms --- cloisim_ros_bringup/src/bringup.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/cloisim_ros_bringup/src/bringup.cpp b/cloisim_ros_bringup/src/bringup.cpp index 317d023a..8d6257d4 100644 --- a/cloisim_ros_bringup/src/bringup.cpp +++ b/cloisim_ros_bringup/src/bringup.cpp @@ -227,7 +227,10 @@ void make_bringup_list( if (!target_model.empty() && target_model.compare(item_name) != 0) continue; else + { parse_target_model(item_list, item_name, target_parts_type, target_parts_name); + this_thread::sleep_for(100ms); + } } // for (auto& it : loaded_key_list) From 0729a679c2958a94252f90f46d4ca7ecfed8b19e Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 14 Aug 2023 18:23:13 +0900 Subject: [PATCH 3/9] modify recv timeout in bridge 1500 -> 3000 ms --- .../include/cloisim_ros_bridge_zmq/bridge.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp index 3fb367c9..c96d01f3 100644 --- a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp +++ b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp @@ -62,7 +62,7 @@ class Bridge const int reconnect_ivl_min = 1000; const int reconnect_ivl_max = 5000; const int lingerPeriod = 0; - const int recv_timeout = 1500; // milliseconds + const int recv_timeout = 3000; // milliseconds std::string bridgeAddr_; From 93fbf07e828f39755c6fca68d1d0e4cb704f90de Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Sun, 27 Aug 2023 15:55:50 +0900 Subject: [PATCH 4/9] Change QoS for clock --- cloisim_ros_world/src/world.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cloisim_ros_world/src/world.cpp b/cloisim_ros_world/src/world.cpp index 5976ff93..9c3de37a 100644 --- a/cloisim_ros_world/src/world.cpp +++ b/cloisim_ros_world/src/world.cpp @@ -49,7 +49,7 @@ void World::Initialize() // Offer transient local durability on the clock topic so that if publishing is infrequent, // late subscribers can receive the previously published message(s). - pub_ = create_publisher("/clock", rclcpp::QoS(rclcpp::KeepLast(10)).transient_local()); + pub_ = create_publisher("/clock", rclcpp::ClockQoS()); auto data_bridge_ptr = CreateBridge(); if (data_bridge_ptr != nullptr) From cbb1971a2e07d815b66aed09a90489e1cefbf543 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 28 Aug 2023 09:31:02 +0900 Subject: [PATCH 5/9] Move Microm::GetStaticTransforms() -> Base::GetStaticTransforms() Add info channel in cloisim_ros_joint_control - handling static TF --- .../include/cloisim_ros_base/base.hpp | 2 + cloisim_ros_base/src/base.cpp | 39 +++++++++ .../src/joint_control.cpp | 14 ++- .../include/cloisim_ros_micom/micom.hpp | 87 +++++++++---------- cloisim_ros_micom/src/micom.cpp | 39 --------- 5 files changed, 95 insertions(+), 86 deletions(-) diff --git a/cloisim_ros_base/include/cloisim_ros_base/base.hpp b/cloisim_ros_base/include/cloisim_ros_base/base.hpp index 9a731809..bd036852 100644 --- a/cloisim_ros_base/include/cloisim_ros_base/base.hpp +++ b/cloisim_ros_base/include/cloisim_ros_base/base.hpp @@ -73,6 +73,8 @@ class Base : public rclcpp::Node void PublishTF(); void PublishTF(const geometry_msgs::msg::TransformStamped& tf); + void GetStaticTransforms(zmq::Bridge *const bridge_ptr); + 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); diff --git a/cloisim_ros_base/src/base.cpp b/cloisim_ros_base/src/base.cpp index 9fd072a4..c56f1a98 100644 --- a/cloisim_ros_base/src/base.cpp +++ b/cloisim_ros_base/src/base.cpp @@ -187,6 +187,45 @@ string Base::GetRobotName() return (is_single_mode) ? robotName : string(get_namespace()).substr(1); } +void Base::GetStaticTransforms(zmq::Bridge* const bridge_ptr) +{ + if (bridge_ptr == nullptr) + { + return; + } + + const auto reply = RequestReplyMessage(bridge_ptr, "request_static_transforms"); + + if (reply.IsInitialized() && + (reply.name().compare("static_transforms") == 0)) + { + auto pose = cloisim::msgs::Pose(); + for (auto link : reply.children()) + { + if (link.IsInitialized() && link.has_name() && link.has_value()) + { + const auto parent_frame_id = (link.value().type() == msgs::Any_ValueType_STRING && link.name().compare("parent_frame_id") == 0) ? link.value().string_value() : "base_link"; + + if (link.children_size() == 1) + { + const auto child = link.children(0); + + if (child.has_name() && child.has_value()) + { + if ((child.name().compare("pose") == 0) && child.value().type() == msgs::Any_ValueType_POSE3D) + { + pose = child.value().pose3d_value(); + } + } + } + + SetStaticTf2(pose, parent_frame_id); + DBG_SIM_MSG("static transform %s -> %s", pose.name().c_str(), parent_frame_id.c_str()); + } + } + } +} + msgs::Pose Base::GetObjectTransform(zmq::Bridge* const bridge_ptr, const string target_name, string& parent_frame_id) { msgs::Pose transform; diff --git a/cloisim_ros_joint_control/src/joint_control.cpp b/cloisim_ros_joint_control/src/joint_control.cpp index 47c4ece1..5c3524c6 100644 --- a/cloisim_ros_joint_control/src/joint_control.cpp +++ b/cloisim_ros_joint_control/src/joint_control.cpp @@ -45,17 +45,25 @@ JointControl::~JointControl() void JointControl::Initialize() { - uint16_t portTx, portRx, portTf; + uint16_t portInfo, portTx, portRx, portTf; + get_parameter_or("bridge.Info", portInfo, uint16_t(0)); get_parameter_or("bridge.Tx", portTx, uint16_t(0)); get_parameter_or("bridge.Rx", portRx, uint16_t(0)); get_parameter_or("bridge.Tf", portTf, uint16_t(0)); - // const auto hashKeyInfo = GetTargetHashKey("Info"); + const auto hashKeyInfo = GetTargetHashKey("Info"); const auto hashKeyPub = GetTargetHashKey("Rx"); const auto hashKeySub = GetTargetHashKey("Tx"); const auto hashKeyTf = GetTargetHashKey("Tf"); DBG_SIM_INFO("hashKey: pub(%s) sub(%s) tf(%s)", hashKeyPub.c_str(), hashKeySub.c_str(), hashKeyTf.c_str()); + auto info_bridge_ptr = CreateBridge(); + if (info_bridge_ptr != nullptr) + { + info_bridge_ptr->Connect(zmq::Bridge::Mode::CLIENT, portInfo, hashKeyInfo); + GetStaticTransforms(info_bridge_ptr); + } + auto data_bridge_ptr = CreateBridge(); if (data_bridge_ptr != nullptr) { @@ -82,7 +90,7 @@ void JointControl::Initialize() // DBG_SIM_INFO("%s %f %f", joint_name.c_str(), displacement, velocity); const auto msgBuf = MakeCommandMessage(joint_name, displacement, velocity); SetBufferToSimulator(data_bridge_ptr, msgBuf); - rclcpp::sleep_for(5ms); + rclcpp::sleep_for(500us); } }; diff --git a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp index 6410290e..853fa94b 100644 --- a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp +++ b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp @@ -16,69 +16,68 @@ #define _CLOISIM_ROS_MICOM_HPP__ #include -#include -#include #include +#include +#include #include #include namespace cloisim_ros { - class Micom : public Base - { - public: - explicit Micom(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit Micom(const std::string namespace_ = ""); - virtual ~Micom(); +class Micom : public Base +{ + public: + explicit Micom(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit Micom(const std::string namespace_ = ""); + virtual ~Micom(); - private: - void Initialize() override; - void Deinitialize() override { }; + private: + void Initialize() override; + void Deinitialize() override{}; - private: - void GetStaticTransforms(zmq::Bridge *const bridge_ptr); - void PublishData(const std::string &buffer); + private: + void PublishData(const std::string &buffer); - void ResetOdometryCallback( - const std::shared_ptr /*request_header*/, - const std::shared_ptr /*request*/, - std::shared_ptr /*response*/); + void ResetOdometryCallback( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr /*response*/); - std::string MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) const; + std::string MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) const; - bool CalculateOdometry(const rclcpp::Duration duration, const double _wheel_angular_vel_left, const double _wheel_angular_vel_right, const double _theta); + bool CalculateOdometry(const rclcpp::Duration duration, const double _wheel_angular_vel_left, const double _wheel_angular_vel_right, const double _theta); - void UpdateOdom(); - void UpdateImu(); - void UpdateBattery(); + void UpdateOdom(); + void UpdateImu(); + void UpdateBattery(); - private: - zmq::Bridge *info_bridge_ptr; + private: + zmq::Bridge *info_bridge_ptr; - // Micom msgs - cloisim::msgs::Micom pb_micom_; + // Micom msgs + cloisim::msgs::Micom pb_micom_; - // IMU msgs - sensor_msgs::msg::Imu msg_imu_; + // IMU msgs + sensor_msgs::msg::Imu msg_imu_; - geometry_msgs::msg::TransformStamped odom_tf_; + geometry_msgs::msg::TransformStamped odom_tf_; - // Odometry msgs - nav_msgs::msg::Odometry msg_odom_; + // Odometry msgs + nav_msgs::msg::Odometry msg_odom_; - // Battery - sensor_msgs::msg::BatteryState msg_battery_; + // Battery + sensor_msgs::msg::BatteryState msg_battery_; - // ROS2 micom publisher - rclcpp::Publisher::SharedPtr pub_imu_; - rclcpp::Publisher::SharedPtr pub_odom_; - rclcpp::Publisher::SharedPtr pub_battery_; + // ROS2 micom publisher + rclcpp::Publisher::SharedPtr pub_imu_; + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Publisher::SharedPtr pub_battery_; - // wheel command subscriber - rclcpp::Subscription::SharedPtr sub_micom_; + // wheel command subscriber + rclcpp::Subscription::SharedPtr sub_micom_; - // reset odometry pose service - rclcpp::Service::SharedPtr srv_reset_odom_; - }; -} + // reset odometry pose service + rclcpp::Service::SharedPtr srv_reset_odom_; +}; +} // namespace cloisim_ros #endif diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp index 8757457a..f8420e51 100644 --- a/cloisim_ros_micom/src/micom.cpp +++ b/cloisim_ros_micom/src/micom.cpp @@ -145,45 +145,6 @@ string Micom::MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) return message; } -void Micom::GetStaticTransforms(zmq::Bridge *const bridge_ptr) -{ - if (bridge_ptr == nullptr) - { - return; - } - - const auto reply = RequestReplyMessage(bridge_ptr, "request_static_transforms"); - - if (reply.IsInitialized() && - (reply.name().compare("static_transforms") == 0)) - { - auto pose = cloisim::msgs::Pose(); - for (auto link : reply.children()) - { - if (link.IsInitialized() && link.has_name() && link.has_value()) - { - const auto parent_frame_id = (link.value().type() == msgs::Any_ValueType_STRING && link.name().compare("parent_frame_id") == 0) ? link.value().string_value() : "base_link"; - - if (link.children_size() == 1) - { - const auto child = link.children(0); - - if (child.has_name() && child.has_value()) - { - if ((child.name().compare("pose") == 0) && child.value().type() == msgs::Any_ValueType_POSE3D) - { - pose = child.value().pose3d_value(); - } - } - } - - SetStaticTf2(pose, parent_frame_id); - DBG_SIM_MSG("static transform %s -> %s", pose.name().c_str(), parent_frame_id.c_str()); - } - } - } -} - void Micom::PublishData(const string &buffer) { if (!pb_micom_.ParseFromString(buffer)) From b2f0b7c8d7843b2260b57f7fd537528a46103fe7 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 28 Aug 2023 15:52:20 +0900 Subject: [PATCH 6/9] Modify code related to ament_cpplint --- .../include/cloisim_ros_actor/actor.hpp | 65 ++++--- cloisim_ros_actor/src/actor.cpp | 17 +- cloisim_ros_actor/src/main.cpp | 2 +- .../include/cloisim_ros_base/base.hpp | 72 ++++--- .../include/cloisim_ros_base/camera_helper.h | 17 +- .../include/cloisim_ros_base/helper.h | 32 ++-- cloisim_ros_base/src/base.cpp | 67 +++++-- .../include/cloisim_ros_bridge_zmq/bridge.hpp | 18 +- .../include/cloisim_ros_bridge_zmq/log.h | 6 +- .../cloisim_ros_bridge_zmq/term_color.h | 40 ++-- cloisim_ros_bridge_zmq/src/bridge.cpp | 10 +- cloisim_ros_bringup/src/bringup.cpp | 31 ++- cloisim_ros_bringup/src/main.cpp | 5 +- .../bringup_param.hpp | 16 +- .../src/bringup_param.cpp | 15 +- .../include/cloisim_ros_camera/camera.hpp | 74 ++++---- cloisim_ros_camera/src/camera.cpp | 15 +- cloisim_ros_camera/src/main.cpp | 2 +- .../cloisim_ros_depthcamera/depthcamera.hpp | 58 +++--- cloisim_ros_depthcamera/src/depthcamera.cpp | 8 +- .../elevator_system.hpp | 178 ++++++++++-------- .../src/elevator_system.cpp | 15 +- cloisim_ros_elevator_system/src/main.cpp | 2 +- .../include/cloisim_ros_gps/gps.hpp | 61 +++--- cloisim_ros_gps/src/gps.cpp | 10 +- .../cloisim_ros_ground_truth/ground_truth.hpp | 45 ++--- cloisim_ros_ground_truth/src/ground_truth.cpp | 7 +- .../include/cloisim_ros_imu/imu.hpp | 11 +- cloisim_ros_imu/src/imu.cpp | 8 +- .../joint_control.hpp | 60 +++--- .../src/joint_control.cpp | 30 ++- cloisim_ros_joint_control/src/main.cpp | 4 +- .../include/cloisim_ros_lidar/lidar.hpp | 80 ++++---- cloisim_ros_lidar/src/lidar.cpp | 13 +- .../include/cloisim_ros_micom/micom.hpp | 12 +- cloisim_ros_micom/src/main.cpp | 4 +- cloisim_ros_micom/src/micom.cpp | 39 ++-- .../cloisim_ros_multicamera/multicamera.hpp | 74 ++++---- cloisim_ros_multicamera/src/multicamera.cpp | 19 +- .../cloisim_ros_realsense/realsense.hpp | 73 +++---- cloisim_ros_realsense/src/main.cpp | 2 +- cloisim_ros_realsense/src/realsense.cpp | 48 +++-- .../include/cloisim_ros_sonar/sonar.hpp | 11 +- cloisim_ros_sonar/src/sonar.cpp | 10 +- .../websocket_service.hpp | 22 +-- .../src/websocket_service.cpp | 7 +- .../include/cloisim_ros_world/world.hpp | 13 +- cloisim_ros_world/src/world.cpp | 6 +- 48 files changed, 857 insertions(+), 577 deletions(-) diff --git a/cloisim_ros_actor/include/cloisim_ros_actor/actor.hpp b/cloisim_ros_actor/include/cloisim_ros_actor/actor.hpp index 08734b6a..ea7de6b2 100644 --- 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 #include + +#include +#include + +#include #include 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::SharedPtr srvCallMoveActor_; - - private: - void CallMoveActor(const std::shared_ptr /*request_header*/, - const std::shared_ptr request, - const std::shared_ptr 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 \ No newline at end of file +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::SharedPtr srvCallMoveActor_; + + private: + void CallMoveActor(const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr 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/src/actor.cpp b/cloisim_ros_actor/src/actor.cpp index e0552567..d37ee509 100644 --- 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(nodeName + "/move_actor", bind(&Actor::CallMoveActor, this, _1, _2, _3)); + srvCallMoveActor_ = this->create_service( + 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; -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_actor/src/main.cpp b/cloisim_ros_actor/src/main.cpp index 2a8a71e7..80d4dfed 100644 --- 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; -} \ No newline at end of file +} diff --git a/cloisim_ros_base/include/cloisim_ros_base/base.hpp b/cloisim_ros_base/include/cloisim_ros_base/base.hpp index bd036852..144e562d 100644 --- a/cloisim_ros_base/include/cloisim_ros_base/base.hpp +++ b/cloisim_ros_base/include/cloisim_ros_base/base.hpp @@ -13,22 +13,34 @@ * 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 -#include -#include #include #include #include #include #include +#include +#include +#include + +#include +#include + 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 { @@ -36,7 +48,8 @@ class Base : public rclcpp::Node 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: @@ -44,9 +57,17 @@ class Base : public rclcpp::Node 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 thread_func); + void AddPublisherThread(zmq::Bridge* const bridge_ptr, + std::function thread_func); std::string GetModelName(); std::string GetRobotName(); @@ -73,22 +95,30 @@ class Base : public rclcpp::Node void PublishTF(); void PublishTF(const geometry_msgs::msg::TransformStamped& tf); - void GetStaticTransforms(zmq::Bridge *const bridge_ptr); + void GetStaticTransforms(zmq::Bridge* const bridge_ptr); - 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 = ""); - 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, + 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); @@ -140,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); @@ -176,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 \ No newline at end of file +#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 index 23b05bf9..c8d5c782 100644 --- 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 +#include + +#include +#include #include #include #include -#include -#include 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 \ No newline at end of file +#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 index 427dc32e..270ee69a 100644 --- a/cloisim_ros_base/include/cloisim_ros_base/helper.h +++ b/cloisim_ros_base/include/cloisim_ros_base/helper.h @@ -13,37 +13,42 @@ * 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 -#include -#include #include +#include #include -#include +#include -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(); @@ -51,7 +56,8 @@ static void SetQuaternionMessageToGeometry(const cloisim::msgs::Quaternion &src, 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(); @@ -59,12 +65,12 @@ static void SetTf2QuaternionToGeometry(const tf2::Quaternion &src, geometry_msgs 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 \ No newline at end of file +#endif // CLOISIM_ROS_BASE__HELPER_H_ diff --git a/cloisim_ros_base/src/base.cpp b/cloisim_ros_base/src/base.cpp index c56f1a98..91438d33 100644 --- a/cloisim_ros_base/src/base.cpp +++ b/cloisim_ros_base/src/base.cpp @@ -13,13 +13,16 @@ * SPDX-License-Identifier: MIT */ +#include + #include "cloisim_ros_base/base.hpp" #include "cloisim_ros_base/helper.h" -#include using namespace std; using namespace cloisim; -using namespace cloisim_ros; + +namespace cloisim_ros +{ Base::Base(const string node_name, const rclcpp::NodeOptions& options) : Base(node_name, "", rclcpp::NodeOptions(options)) @@ -42,7 +45,9 @@ Base::Base(const string node_name, const string namespace_, const rclcpp::NodeOp rclcpp::NodeOptions(options) .automatically_declare_parameters_from_overrides(true) .append_parameter_override("use_sim_time", true) - .arguments(vector{"--ros-args", "--remap", "/tf:=tf", "--remap", "/tf_static:=tf_static"})) + .arguments(vector{"--ros-args", + "--remap", "/tf:=tf", + "--remap", "/tf_static:=tf_static"})) , m_bRunThread(false) , m_node_handle(shared_ptr(this, [](auto) {})) , m_static_tf_broadcaster(nullptr) @@ -109,7 +114,10 @@ void Base::GenerateTF(const string& buffer) newTf.header.frame_id = pb_transform_stamped.header().str_id(); newTf.child_frame_id = pb_transform_stamped.transform().name(); // DBG_SIM_INFO("%ld %ld %s %s", newTf.header.stamp.sec, newTf.header.stamp.nanosec, newTf.header.frame_id.c_str(), newTf.child_frame_id.c_str()); - SetTf2(newTf, pb_transform_stamped.transform(), pb_transform_stamped.transform().name(), pb_transform_stamped.header().str_id()); + SetTf2(newTf, + pb_transform_stamped.transform(), + pb_transform_stamped.transform().name(), + pb_transform_stamped.header().str_id()); PublishTF(newTf); } else @@ -147,7 +155,9 @@ void Base::CloseBridges() bridge->Disconnect(); } -void Base::AddPublisherThread(zmq::Bridge* const bridge_ptr, function thread_func) +void Base::AddPublisherThread( + zmq::Bridge* const bridge_ptr, + function thread_func) { m_threads.emplace_back( [this, bridge_ptr, thread_func]() @@ -204,7 +214,10 @@ void Base::GetStaticTransforms(zmq::Bridge* const bridge_ptr) { if (link.IsInitialized() && link.has_name() && link.has_value()) { - const auto parent_frame_id = (link.value().type() == msgs::Any_ValueType_STRING && link.name().compare("parent_frame_id") == 0) ? link.value().string_value() : "base_link"; + const auto parent_frame_id = (link.value().type() == msgs::Any_ValueType_STRING && + link.name().compare("parent_frame_id") == 0) + ? link.value().string_value() + : "base_link"; if (link.children_size() == 1) { @@ -212,7 +225,8 @@ void Base::GetStaticTransforms(zmq::Bridge* const bridge_ptr) if (child.has_name() && child.has_value()) { - if ((child.name().compare("pose") == 0) && child.value().type() == msgs::Any_ValueType_POSE3D) + if ((child.name().compare("pose") == 0) && + child.value().type() == msgs::Any_ValueType_POSE3D) { pose = child.value().pose3d_value(); } @@ -226,7 +240,10 @@ void Base::GetStaticTransforms(zmq::Bridge* const bridge_ptr) } } -msgs::Pose Base::GetObjectTransform(zmq::Bridge* const bridge_ptr, const string target_name, string& parent_frame_id) +msgs::Pose Base::GetObjectTransform( + zmq::Bridge* const bridge_ptr, + const string target_name, + string& parent_frame_id) { msgs::Pose transform; transform.Clear(); @@ -278,7 +295,10 @@ void Base::GetRos2Parameter(zmq::Bridge* const bridge_ptr) for (auto i = 0; i < reply.children_size(); i++) { const auto param = reply.children(i); - const auto paramValue = (param.has_value() && param.value().type() == msgs::Any_ValueType_STRING) ? param.value().string_value() : ""; + const auto paramValue = (param.has_value() && + param.value().type() == msgs::Any_ValueType_STRING) + ? param.value().string_value() + : ""; if (param.name().compare("topic_name") == 0) { @@ -296,7 +316,11 @@ void Base::GetRos2Parameter(zmq::Bridge* const bridge_ptr) } } -bool Base::GetBufferFromSimulator(zmq::Bridge* const bridge_ptr, void** ppBbuffer, int& bufferLength, const bool isNonBlockingMode) +bool Base::GetBufferFromSimulator( + zmq::Bridge* const bridge_ptr, + void** ppBbuffer, + int& bufferLength, + const bool isNonBlockingMode) { if (bridge_ptr == nullptr) { @@ -314,7 +338,11 @@ bool Base::GetBufferFromSimulator(zmq::Bridge* const bridge_ptr, void** ppBbuffe return true; } -void Base::SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const msgs::Pose transform, const string child_frame_id, const string header_frame_id) +void Base::SetTf2( + geometry_msgs::msg::TransformStamped& target_msg, + const msgs::Pose transform, + const string child_frame_id, + const string header_frame_id) { target_msg.header.frame_id = header_frame_id; target_msg.child_frame_id = child_frame_id; @@ -322,7 +350,9 @@ void Base::SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const msgs:: SetQuaternionMessageToGeometry(transform.orientation(), target_msg.transform.rotation); } -void Base::SetStaticTf2(const msgs::Pose transform, const string parent_header_frame_id) +void Base::SetStaticTf2( + const msgs::Pose transform, + const string parent_header_frame_id) { geometry_msgs::msg::TransformStamped static_tf; static_tf.header.frame_id = parent_header_frame_id; @@ -333,7 +363,10 @@ void Base::SetStaticTf2(const msgs::Pose transform, const string parent_header_f AddStaticTf2(static_tf); } -msgs::Param Base::RequestReplyMessage(zmq::Bridge* const bridge_ptr, const string request_message, const string request_value) +msgs::Param Base::RequestReplyMessage( + zmq::Bridge* const bridge_ptr, + const string request_message, + const string request_value) { msgs::Param reply; @@ -369,7 +402,9 @@ msgs::Param Base::RequestReplyMessage(zmq::Bridge* const bridge_ptr, const strin return reply; } -msgs::Param Base::RequestReplyMessage(zmq::Bridge* const bridge_ptr, const msgs::Param request_message) +msgs::Param Base::RequestReplyMessage( + zmq::Bridge* const bridge_ptr, + const msgs::Param request_message) { msgs::Param response_msg; @@ -396,4 +431,6 @@ msgs::Pose Base::IdentityPose() identityTransform.mutable_orientation()->set_z(0.0); identityTransform.mutable_orientation()->set_w(1.0); return identityTransform; -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp index c96d01f3..5813b8b2 100644 --- a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp +++ b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp @@ -13,12 +13,14 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_BRIDGE_ZMQ_HPP_ -#define _CLOISIM_ROS_BRIDGE_ZMQ_HPP_ +#ifndef CLOISIM_ROS_BRIDGE_ZMQ__BRIDGE_HPP_ +#define CLOISIM_ROS_BRIDGE_ZMQ__BRIDGE_HPP_ + +#include #include "log.h" + #include -#include namespace cloisim_ros { @@ -76,8 +78,8 @@ class Bridge zmq_msg_t m_msgRx; // for subscriber and reply std::size_t m_nHashTagTx; // for publisher and request - void* pSockTx_; // for Send function - void* pSockRx_; // for Recieve function + void* pSockTx_; // for Send function + void* pSockRx_; // for Recieve function std::string lastErrMsg; @@ -89,7 +91,7 @@ class Bridge bool SetupService(); bool SetupClient(); - // TODO : need to implement action/client model later + // TODO(@hyunseok-yang) : need to implement action/client model later // bool SetupAction() { return false; }; // bool SetupClient() { return false; }; @@ -98,7 +100,7 @@ class Bridge bool ConnectService(const uint16_t port, const std::string hashKey); bool ConnectClient(const uint16_t port, const std::string hashKey); - // TODO : need to implement action/client model later + // TODO(@hyunseok-yang) : need to implement action/client model later // bool ConnectAction() { return false; }; // bool ConnectClient() { return false; }; @@ -117,4 +119,4 @@ class Bridge }; } // namespace zmq } // namespace cloisim_ros -#endif \ No newline at end of file +#endif // CLOISIM_ROS_BRIDGE_ZMQ__BRIDGE_HPP_ diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h index beb8b68e..0edcd58f 100644 --- a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h +++ b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h @@ -12,8 +12,8 @@ * * SPDX-License-Identifier: MIT */ -#ifndef _LOG_H_ -#define _LOG_H_ +#ifndef CLOISIM_ROS_BRIDGE_ZMQ__LOG_H_ +#define CLOISIM_ROS_BRIDGE_ZMQ__LOG_H_ #include "term_color.h" @@ -28,4 +28,4 @@ #define DBG_SIM_MSG(STR_FORMAT, ...) __SIM_LOG(GREEN, STR_FORMAT, ##__VA_ARGS__) #define DBG_SIM_ERR(STR_FORMAT, ...) __SIM_LOG(BOLDRED, STR_FORMAT, ##__VA_ARGS__) -#endif \ No newline at end of file +#endif // CLOISIM_ROS_BRIDGE_ZMQ__LOG_H_ diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/term_color.h b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/term_color.h index cd0479f8..46ac22ff 100644 --- a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/term_color.h +++ b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/term_color.h @@ -13,25 +13,25 @@ * SPDX-License-Identifier: MIT */ -#ifndef _TERM_COLOR_H_ -#define _TERM_COLOR_H_ +#ifndef CLOISIM_ROS_BRIDGE_ZMQ__TERM_COLOR_H_ +#define CLOISIM_ROS_BRIDGE_ZMQ__TERM_COLOR_H_ -#define RESET "\033[0m" -#define BLACK "\033[30m" /* Black */ -#define RED "\033[31m" /* Red */ -#define GREEN "\033[32m" /* Green */ -#define YELLOW "\033[33m" /* Yellow */ -#define BLUE "\033[34m" /* Blue */ -#define MAGENTA "\033[35m" /* Magenta */ -#define CYAN "\033[36m" /* Cyan */ -#define WHITE "\033[37m" /* White */ -#define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ -#define BOLDRED "\033[1m\033[31m" /* Bold Red */ -#define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ -#define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ -#define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ -#define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ -#define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ -#define BOLDWHITE "\033[1m\033[37m" /* Bold White */ +#define RESET "\033[0m" +#define BLACK "\033[30m" /* Black */ +#define RED "\033[31m" /* Red */ +#define GREEN "\033[32m" /* Green */ +#define YELLOW "\033[33m" /* Yellow */ +#define BLUE "\033[34m" /* Blue */ +#define MAGENTA "\033[35m" /* Magenta */ +#define CYAN "\033[36m" /* Cyan */ +#define WHITE "\033[37m" /* White */ +#define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ +#define BOLDRED "\033[1m\033[31m" /* Bold Red */ +#define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ +#define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ +#define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ +#define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ +#define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ +#define BOLDWHITE "\033[1m\033[37m" /* Bold White */ -#endif \ No newline at end of file +#endif // CLOISIM_ROS_BRIDGE_ZMQ__TERM_COLOR_H_ diff --git a/cloisim_ros_bridge_zmq/src/bridge.cpp b/cloisim_ros_bridge_zmq/src/bridge.cpp index d87b1d6a..15a88744 100644 --- a/cloisim_ros_bridge_zmq/src/bridge.cpp +++ b/cloisim_ros_bridge_zmq/src/bridge.cpp @@ -18,10 +18,12 @@ #include using namespace std; -using namespace cloisim_ros::zmq; #define DEFAULT_CLOISIM_BRIDGE_IP "127.0.0.1" +namespace cloisim_ros::zmq +{ + Bridge::Bridge() : pCtx_(nullptr) , pPub_(nullptr) @@ -419,7 +421,7 @@ bool Bridge::Receive(void **buffer, int &bufferLength, bool isNonBlockingMode) // Get only Contents without tag auto ptr = static_cast(*buffer); - *buffer = (void *)(ptr + tagSize); + *buffer = reinterpret_cast(ptr + tagSize); bufferLength -= tagSize; return true; @@ -436,7 +438,7 @@ bool Bridge::Send(const void *buffer, const int bufferLength, bool isNonBlocking // Set hash Tag memcpy(zmq_msg_data(&msg), &m_nHashTagTx, tagSize); - memcpy((void *)((uint8_t *)zmq_msg_data(&msg) + tagSize), buffer, bufferLength); + memcpy(reinterpret_cast(reinterpret_cast(zmq_msg_data(&msg)) + tagSize), buffer, bufferLength); /* Send the message to the socket */ if (zmq_msg_send(&msg, pSockTx_, (isNonBlockingMode) ? ZMQ_DONTWAIT : 0) < 0) @@ -469,3 +471,5 @@ std::string Bridge::RequestReply(std::string request_data) return reply_data; } + +} // namespace cloisim_ros::zmq diff --git a/cloisim_ros_bringup/src/bringup.cpp b/cloisim_ros_bringup/src/bringup.cpp index 8d6257d4..7f9f4af4 100644 --- a/cloisim_ros_bringup/src/bringup.cpp +++ b/cloisim_ros_bringup/src/bringup.cpp @@ -36,7 +36,11 @@ map, tuple> loaded_key_list; static bool enable_single_mode = false; -static shared_ptr make_device_node(rclcpp::NodeOptions& node_options, const string& node_type, const string& model_name, const string& node_name) +static shared_ptr make_device_node( + rclcpp::NodeOptions& node_options, + const string& node_type, + const string& model_name, + const string& node_name) { shared_ptr node = nullptr; @@ -113,11 +117,18 @@ static shared_ptr make_device_node(rclcpp::NodeOptions& node_ else node = std::make_shared(node_options, node_name, model_name); } + else + { + } return node; } -static shared_ptr make_world_node(rclcpp::NodeOptions& node_options, const string& node_type, const string& model_name, const string& node_name) +static shared_ptr make_world_node( + rclcpp::NodeOptions& node_options, + const string& node_type, + const string& model_name, + const string& node_name) { shared_ptr node = nullptr; @@ -135,12 +146,16 @@ static shared_ptr make_world_node(rclcpp::NodeOptions& node_o return node; } -static void parse_target_parts_by_name(const Json::Value item, const string node_type, const string model_name, const string node_name) +static void parse_target_parts_by_name( + const Json::Value item, + const string node_type, + const string model_name, + const string node_name) { shared_ptr node = nullptr; rclcpp::NodeOptions node_options; - node_options.append_parameter_override("single_mode", bool(enable_single_mode)); + node_options.append_parameter_override("single_mode", static_cast(enable_single_mode)); cloisim_ros::BringUpParam::StoreBridgeInfosAsParameters(item, node_options); const auto node_info = "Target Model/Type/Parts: " + model_name + "/" + node_type + "/" + node_name; @@ -170,7 +185,11 @@ static void parse_target_parts_by_name(const Json::Value item, const string node } } -static void parse_target_parts_by_type(const Json::Value node_list, const string node_type, const string model_name, const string targetPartsName) +static void parse_target_parts_by_type( + const Json::Value node_list, + const string node_type, + const string model_name, + const string targetPartsName) { // cout << "\tNode Type(Target Parts Type): " << node_type << endl; @@ -252,4 +271,4 @@ void make_bringup_list( get<1>(value) = true; // mark to remove } } -} \ No newline at end of file +} diff --git a/cloisim_ros_bringup/src/main.cpp b/cloisim_ros_bringup/src/main.cpp index bbc73e78..8886e4ef 100644 --- a/cloisim_ros_bringup/src/main.cpp +++ b/cloisim_ros_bringup/src/main.cpp @@ -12,9 +12,10 @@ * SPDX-License-Identifier: MIT */ +#include + #include #include -#include using namespace std; @@ -160,4 +161,4 @@ int main(int argc, char** argv) rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp b/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp index fc645c52..ea75bd3d 100644 --- a/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp +++ b/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp @@ -14,21 +14,25 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_BRINGUP_PARAM_HPP_ -#define _CLOISIM_ROS_BRINGUP_PARAM_HPP_ +#ifndef CLOISIM_ROS_BRINGUP_PARAM__BRINGUP_PARAM_HPP_ +#define CLOISIM_ROS_BRINGUP_PARAM__BRINGUP_PARAM_HPP_ + +#include + +#include #include #include -#include + +using namespace std; namespace cloisim_ros { -using namespace std; class BringUpParam : public rclcpp::Node { public: - BringUpParam(const string basename = "cloisim_ros"); + explicit BringUpParam(const string basename = "cloisim_ros"); virtual ~BringUpParam(); bool IsSingleMode() const { return is_single_mode; } @@ -66,4 +70,4 @@ class BringUpParam : public rclcpp::Node Json::Value GetFilteredListByParameters(const Json::Value result); }; } // namespace cloisim_ros -#endif \ No newline at end of file +#endif // CLOISIM_ROS_BRINGUP_PARAM__BRINGUP_PARAM_HPP_ diff --git a/cloisim_ros_bringup_param/src/bringup_param.cpp b/cloisim_ros_bringup_param/src/bringup_param.cpp index 1754fe27..86e2f6f9 100644 --- a/cloisim_ros_bringup_param/src/bringup_param.cpp +++ b/cloisim_ros_bringup_param/src/bringup_param.cpp @@ -14,7 +14,8 @@ #include "cloisim_ros_bringup_param/bringup_param.hpp" -using namespace cloisim_ros; +namespace cloisim_ros +{ bool BringUpParam::IsRobotSpecificType(const string node_type) { @@ -158,7 +159,9 @@ Json::Value BringUpParam::GetBringUpList(const bool filterByParameters) return (filterByParameters) ? GetFilteredListByParameters(bringup_map_list) : bringup_map_list; } -void BringUpParam::StoreBridgeInfosAsParameters(const Json::Value item, rclcpp::NodeOptions &node_options) +void BringUpParam::StoreBridgeInfosAsParameters( + const Json::Value item, + rclcpp::NodeOptions &node_options) { if (!item.isNull()) { @@ -172,7 +175,9 @@ void BringUpParam::StoreBridgeInfosAsParameters(const Json::Value item, rclcpp:: } } -void BringUpParam::StoreFilteredInfoAsParameters(const Json::Value item, rclcpp::NodeOptions &node_options) +void BringUpParam::StoreFilteredInfoAsParameters( + const Json::Value item, + rclcpp::NodeOptions &node_options) { // cout << item << endl; // cout << "model_name: " << model_name << ", " << TargetModel() << endl; @@ -197,4 +202,6 @@ void BringUpParam::StoreFilteredInfoAsParameters(const Json::Value item, rclcpp: node_options.append_parameter_override("model", TargetModel()); cloisim_ros::BringUpParam::StoreBridgeInfosAsParameters(bridge_infos, node_options); -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp index 5f4033c7..b438910c 100644 --- a/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp @@ -11,45 +11,49 @@ * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ -#ifndef _CLOISIM_ROS_CAMERA_HPP_ -#define _CLOISIM_ROS_CAMERA_HPP_ +#ifndef CLOISIM_ROS_CAMERA__CAMERA_HPP_ +#define CLOISIM_ROS_CAMERA__CAMERA_HPP_ +#include +#include +#include + +#include +#include + +#include #include #include -#include #include -#include -#include -#include namespace cloisim_ros { - class Camera : public Base - { - public: - explicit Camera(const rclcpp::NodeOptions &options_, const std::string node_name = "cloisim_ros_camera", const std::string namespace_ = ""); - explicit Camera(const std::string node_name = "cloisim_ros_camera", const std::string namespace_ = ""); - virtual ~Camera(); - - protected: - void Initialize() override; - void Deinitialize() override; - - protected: - void PublishData(const std::string &buffer); - - private: - // image buffer from simulator - cloisim::msgs::ImageStamped pb_img_; - - // message for ROS2 communictaion - sensor_msgs::msg::Image msg_img_; - - // Image publisher - image_transport::CameraPublisher pub_; - - // Camera info manager - std::shared_ptr camera_info_manager_; - }; -} -#endif \ No newline at end of file +class Camera : public Base +{ + public: + explicit Camera(const rclcpp::NodeOptions &options_, const std::string node_name = "cloisim_ros_camera", const std::string namespace_ = ""); + explicit Camera(const std::string node_name = "cloisim_ros_camera", const std::string namespace_ = ""); + virtual ~Camera(); + + protected: + void Initialize() override; + void Deinitialize() override; + + protected: + void PublishData(const std::string &buffer); + + private: + // image buffer from simulator + cloisim::msgs::ImageStamped pb_img_; + + // message for ROS2 communictaion + sensor_msgs::msg::Image msg_img_; + + // Image publisher + image_transport::CameraPublisher pub_; + + // Camera info manager + std::shared_ptr camera_info_manager_; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_CAMERA__CAMERA_HPP_ diff --git a/cloisim_ros_camera/src/camera.cpp b/cloisim_ros_camera/src/camera.cpp index 69bdeff4..736f86c7 100644 --- a/cloisim_ros_camera/src/camera.cpp +++ b/cloisim_ros_camera/src/camera.cpp @@ -11,17 +11,19 @@ * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ -#include -#include -#include #include #include #include +#include +#include +#include + using namespace std; -using namespace chrono_literals; -using namespace cloisim_ros; +using namespace std::chrono_literals; +namespace cloisim_ros +{ Camera::Camera(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) : Base(node_name, namespace_, options_) { @@ -114,4 +116,5 @@ void Camera::PublishData(const string &buffer) camera_info_msg.header.stamp = msg_img_.header.stamp; pub_.publish(msg_img_, camera_info_msg); -} \ No newline at end of file +} +} // namespace cloisim_ros diff --git a/cloisim_ros_camera/src/main.cpp b/cloisim_ros_camera/src/main.cpp index d2647556..ff1245f8 100644 --- a/cloisim_ros_camera/src/main.cpp +++ b/cloisim_ros_camera/src/main.cpp @@ -51,4 +51,4 @@ int main(int argc, char** argv) executor.spin(); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_depthcamera/include/cloisim_ros_depthcamera/depthcamera.hpp b/cloisim_ros_depthcamera/include/cloisim_ros_depthcamera/depthcamera.hpp index c514c2c3..af45c7b4 100644 --- a/cloisim_ros_depthcamera/include/cloisim_ros_depthcamera/depthcamera.hpp +++ b/cloisim_ros_depthcamera/include/cloisim_ros_depthcamera/depthcamera.hpp @@ -12,37 +12,39 @@ * All Rights are Reserved. */ -#ifndef _CLOISIM_ROS_DEPTHCAMERA_HPP_ -#define _CLOISIM_ROS_DEPTHCAMERA_HPP_ +#ifndef CLOISIM_ROS_DEPTHCAMERA__DEPTHCAMERA_HPP_ +#define CLOISIM_ROS_DEPTHCAMERA__DEPTHCAMERA_HPP_ + +#include #include #include namespace cloisim_ros { - class DepthCamera : public Camera - { - public: - explicit DepthCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit DepthCamera(const std::string node_name = "cloisim_ros_depthcamera", const std::string namespace_ = ""); - virtual ~DepthCamera(); - - private: - void Initialize() override; - void Deinitialize() override; - - private: - void PublishData(const std::string &buffer); - - private: - // Camera info publisher - // rclcpp::Publisher::SharedPtr pubDepthCameraInfo{nullptr}; - - // Store current point cloud. - // sensor_msgs::msg::PointCloud2 msg_pc2; - - // Point cloud publisher. - // rclcpp::Publisher::SharedPtr pubPointCloud; - }; -} -#endif \ No newline at end of file +class DepthCamera : public Camera +{ + public: + explicit DepthCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit DepthCamera(const std::string node_name = "cloisim_ros_depthcamera", const std::string namespace_ = ""); + virtual ~DepthCamera(); + + private: + void Initialize() override; + void Deinitialize() override; + + private: + void PublishData(const std::string &buffer); + + private: + // Camera info publisher + // rclcpp::Publisher::SharedPtr pubDepthCameraInfo{nullptr}; + + // Store current point cloud. + // sensor_msgs::msg::PointCloud2 msg_pc2; + + // Point cloud publisher. + // rclcpp::Publisher::SharedPtr pubPointCloud; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_DEPTHCAMERA__DEPTHCAMERA_HPP_ diff --git a/cloisim_ros_depthcamera/src/depthcamera.cpp b/cloisim_ros_depthcamera/src/depthcamera.cpp index 14ce6e6e..7a397d7e 100644 --- a/cloisim_ros_depthcamera/src/depthcamera.cpp +++ b/cloisim_ros_depthcamera/src/depthcamera.cpp @@ -15,7 +15,9 @@ #include "cloisim_ros_depthcamera/depthcamera.hpp" using namespace std; -using namespace cloisim_ros; + +namespace cloisim_ros +{ DepthCamera::DepthCamera(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) : Camera(options_, node_name, namespace_) @@ -50,4 +52,6 @@ void DepthCamera::Deinitialize() void DepthCamera::PublishData(const string &buffer) { Camera::PublishData(buffer); -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_elevator_system/include/cloisim_ros_elevator_system/elevator_system.hpp b/cloisim_ros_elevator_system/include/cloisim_ros_elevator_system/elevator_system.hpp index 11f71208..c4632850 100644 --- a/cloisim_ros_elevator_system/include/cloisim_ros_elevator_system/elevator_system.hpp +++ b/cloisim_ros_elevator_system/include/cloisim_ros_elevator_system/elevator_system.hpp @@ -13,93 +13,111 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_ELEVATOR_SYSTEM_HPP_ -#define _CLOISIM_ROS_ELEVATOR_SYSTEM_HPP_ +#ifndef CLOISIM_ROS_ELEVATOR_SYSTEM__ELEVATOR_SYSTEM_HPP_ +#define CLOISIM_ROS_ELEVATOR_SYSTEM__ELEVATOR_SYSTEM_HPP_ -#include #include + +#include +#include + +#include #include #include -#include #include #include +#include namespace cloisim_ros { - class ElevatorSystem : public Base + +class ElevatorSystem : public Base +{ + public: + explicit ElevatorSystem(const rclcpp::NodeOptions &options_, const std::string node_name); + ElevatorSystem(); + virtual ~ElevatorSystem(); + + private: + void Initialize() override; + void Deinitialize() override{}; + + private: + bool srv_mode_; + zmq::Bridge *control_bridge_ptr; + cloisim::msgs::Param request_msg_; + + rclcpp::Service::SharedPtr srvCallElevator_; + rclcpp::Service::SharedPtr srvGetCalledElevator_; + rclcpp::Service::SharedPtr srvGetElevatorInfo_; + rclcpp::Service::SharedPtr srvSelectElevatorFloor_; + rclcpp::Service::SharedPtr srvRequestDoorOpen_; + rclcpp::Service::SharedPtr srvRequestDoorClose_; + rclcpp::Service::SharedPtr srvIsDoorOpened_; + rclcpp::Service::SharedPtr srvReserveElevator_; + rclcpp::Service::SharedPtr srvReleaseElevator_; + + private: + void CallElevator( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + const std::shared_ptr response); + + void GetElevatorCalled( + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response); + + void GetElevatorInfo( + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response); + + void SelectFloor( + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response); + + void RequestDoorOpen( + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response); + + void RequestDoorClose( + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response); + + void IsDoorOpened( + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response); + + void ReserveElevator( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + const std::shared_ptr response); + + void ReleaseElevator( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + const std::shared_ptr response); + + cloisim::msgs::Param CreateRequest( + const std::string service_name, const std::string elevator_index) { - public: - explicit ElevatorSystem(const rclcpp::NodeOptions &options_, const std::string node_name); - explicit ElevatorSystem(); - virtual ~ElevatorSystem(); - - private: - void Initialize() override; - void Deinitialize() override { }; - - private: - bool srv_mode_; - zmq::Bridge *control_bridge_ptr; - cloisim::msgs::Param request_msg_; - - rclcpp::Service::SharedPtr srvCallElevator_; - rclcpp::Service::SharedPtr srvGetCalledElevator_; - rclcpp::Service::SharedPtr srvGetElevatorInfo_; - rclcpp::Service::SharedPtr srvSelectElevatorFloor_; - rclcpp::Service::SharedPtr srvRequestDoorOpen_; - rclcpp::Service::SharedPtr srvRequestDoorClose_; - rclcpp::Service::SharedPtr srvIsDoorOpened_; - rclcpp::Service::SharedPtr srvReserveElevator_; - rclcpp::Service::SharedPtr srvReleaseElevator_; - - private: - void CallElevator(const std::shared_ptr /*request_header*/, - const std::shared_ptr /*request*/, - const std::shared_ptr response); - - void GetElevatorCalled(const std::shared_ptr /*request_header*/, - const std::shared_ptr request, - const std::shared_ptr response); - - void GetElevatorInfo(const std::shared_ptr /*request_header*/, - const std::shared_ptr request, - const std::shared_ptr response); - - void SelectFloor(const std::shared_ptr /*request_header*/, - const std::shared_ptr request, - const std::shared_ptr response); - - void RequestDoorOpen(const std::shared_ptr /*request_header*/, - const std::shared_ptr request, - const std::shared_ptr response); - - void RequestDoorClose(const std::shared_ptr /*request_header*/, - const std::shared_ptr request, - const std::shared_ptr response); - - void IsDoorOpened(const std::shared_ptr /*request_header*/, - const std::shared_ptr request, - const std::shared_ptr response); - - void ReserveElevator(const std::shared_ptr /*request_header*/, - const std::shared_ptr /*request*/, - const std::shared_ptr response); - - void ReleaseElevator(const std::shared_ptr /*request_header*/, - const std::shared_ptr /*request*/, - const std::shared_ptr response); - - cloisim::msgs::Param CreateRequest(const std::string service_name, const std::string elevator_index) - { - return CreateRequest(service_name, "", "", elevator_index); - } - - cloisim::msgs::Param CreateRequest(const std::string service_name, - const std::string current_floor, - const std::string target_floor, - const std::string elevator_index = ""); - - bool GetResultFromResponse(const cloisim::msgs::Param &response_msg, const int children_index = 1); - }; -} -#endif \ No newline at end of file + return CreateRequest(service_name, "", "", elevator_index); + } + + cloisim::msgs::Param CreateRequest( + const std::string service_name, + const std::string current_floor, + const std::string target_floor, + const std::string elevator_index = ""); + + bool GetResultFromResponse( + const cloisim::msgs::Param &response_msg, + const int children_index = 1); +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_ELEVATOR_SYSTEM__ELEVATOR_SYSTEM_HPP_ diff --git a/cloisim_ros_elevator_system/src/elevator_system.cpp b/cloisim_ros_elevator_system/src/elevator_system.cpp index 1b28599a..899c7ff1 100644 --- a/cloisim_ros_elevator_system/src/elevator_system.cpp +++ b/cloisim_ros_elevator_system/src/elevator_system.cpp @@ -16,11 +16,12 @@ #include "cloisim_ros_elevator_system/elevator_system.hpp" using namespace std; -using namespace placeholders; +using namespace std::placeholders; using namespace cloisim; -using namespace cloisim_ros; using namespace elevator_system_msgs; +namespace cloisim_ros +{ #define BindCallback(func) bind(&ElevatorSystem::func, this, _1, _2, _3) ElevatorSystem::ElevatorSystem(const rclcpp::NodeOptions &options_, const string node_name) @@ -157,14 +158,16 @@ void ElevatorSystem::GetElevatorInfo( result_param = reply.children(3); if (result_param.IsInitialized()) { - const auto current_floor = (result_param.name().compare("current_floor") != 0) ? "" : result_param.value().string_value(); + const auto current_floor = + (result_param.name().compare("current_floor") != 0) ? "" : result_param.value().string_value(); response->current_floor = current_floor; } result_param = reply.children(4); if (result_param.IsInitialized()) { - const auto height = (float)((result_param.name().compare("height") != 0) ? 0.0 : result_param.value().double_value()); + const auto height = + static_cast((result_param.name().compare("height") != 0) ? 0.0 : result_param.value().double_value()); response->height = height; } } @@ -302,4 +305,6 @@ bool ElevatorSystem::GetResultFromResponse(const msgs::Param &response_msg, cons } return result_param.value().bool_value(); -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_elevator_system/src/main.cpp b/cloisim_ros_elevator_system/src/main.cpp index f530829c..5afb4e1d 100644 --- a/cloisim_ros_elevator_system/src/main.cpp +++ b/cloisim_ros_elevator_system/src/main.cpp @@ -54,4 +54,4 @@ int main(int argc, char *argv[]) rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp b/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp index 31ee11fb..2cee79f3 100644 --- a/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp +++ b/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp @@ -13,38 +13,41 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_GPS_HPP_ -#define _CLOISIM_ROS_GPS_HPP_ +#ifndef CLOISIM_ROS_GPS__GPS_HPP_ +#define CLOISIM_ROS_GPS__GPS_HPP_ + +#include + +#include #include #include -#include namespace cloisim_ros { - class Gps : public Base - { - public: - explicit Gps(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit Gps(const std::string namespace_ = ""); - ~Gps(); - - private: - void Initialize() override; - void Deinitialize() override { }; - - private: - void PublishData(const std::string &buffer); - - private: - // buffer from simulation - cloisim::msgs::GPS pb_buf_; - - // message for ROS2 communictaion - sensor_msgs::msg::NavSatFix msg_nav_; - - // publisher - rclcpp::Publisher::SharedPtr pub_; - }; -} -#endif \ No newline at end of file +class Gps : public Base +{ + public: + explicit Gps(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit Gps(const std::string namespace_ = ""); + ~Gps(); + + private: + void Initialize() override; + void Deinitialize() override{}; + + private: + void PublishData(const std::string &buffer); + + private: + // buffer from simulation + cloisim::msgs::GPS pb_buf_; + + // message for ROS2 communictaion + sensor_msgs::msg::NavSatFix msg_nav_; + + // publisher + rclcpp::Publisher::SharedPtr pub_; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_GPS__GPS_HPP_ diff --git a/cloisim_ros_gps/src/gps.cpp b/cloisim_ros_gps/src/gps.cpp index 5c6b6e68..ad0caf52 100644 --- a/cloisim_ros_gps/src/gps.cpp +++ b/cloisim_ros_gps/src/gps.cpp @@ -17,7 +17,9 @@ #include using namespace std; -using namespace cloisim_ros; + +namespace cloisim_ros +{ Gps::Gps(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) : Base(node_name, namespace_, options_) @@ -66,7 +68,7 @@ void Gps::Initialize() } // Fill covariances - // TODO: need to applying noise + // TODO(@hyunseok-yang): need to applying noise msg_nav_.position_covariance[0] = 0.0001f; msg_nav_.position_covariance[4] = 0.0001f; msg_nav_.position_covariance[8] = 0.0001f; @@ -103,4 +105,6 @@ void Gps::PublishData(const string &buffer) msg_nav_.altitude = pb_buf_.altitude(); pub_->publish(msg_nav_); -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_ground_truth/include/cloisim_ros_ground_truth/ground_truth.hpp b/cloisim_ros_ground_truth/include/cloisim_ros_ground_truth/ground_truth.hpp index 2cd7192b..f09c0543 100644 --- a/cloisim_ros_ground_truth/include/cloisim_ros_ground_truth/ground_truth.hpp +++ b/cloisim_ros_ground_truth/include/cloisim_ros_ground_truth/ground_truth.hpp @@ -14,35 +14,38 @@ * SPDX-License-Identifier: MIT */ -#ifndef _cloisim_ros_ground_truth_HPP_ -#define _cloisim_ros_ground_truth_HPP_ +#ifndef CLOISIM_ROS_GROUND_TRUTH__GROUND_TRUTH_HPP_ +#define CLOISIM_ROS_GROUND_TRUTH__GROUND_TRUTH_HPP_ -#include #include + +#include + +#include #include namespace cloisim_ros { - class GroundTruth : public Base - { - public: - explicit GroundTruth(const rclcpp::NodeOptions &options_, const std::string node_name); - explicit GroundTruth(); - virtual ~GroundTruth(); +class GroundTruth : public Base +{ + public: + explicit GroundTruth(const rclcpp::NodeOptions &options_, const std::string node_name); + explicit GroundTruth(); + virtual ~GroundTruth(); - private: - void Initialize() override; - void Deinitialize() override { }; + private: + void Initialize() override; + void Deinitialize() override{}; - void PublishData(const std::string &buffer); - void UpdatePerceptionData(); + void PublishData(const std::string &buffer); + void UpdatePerceptionData(); - private: - cloisim::msgs::Perception_V pb_buf_; + private: + cloisim::msgs::Perception_V pb_buf_; - perception_msgs::msg::ObjectArray msg_; + perception_msgs::msg::ObjectArray msg_; - rclcpp::Publisher::SharedPtr pub_; - }; -} -#endif \ No newline at end of file + rclcpp::Publisher::SharedPtr pub_; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_GROUND_TRUTH__GROUND_TRUTH_HPP_ diff --git a/cloisim_ros_ground_truth/src/ground_truth.cpp b/cloisim_ros_ground_truth/src/ground_truth.cpp index 984174de..a55dfbe6 100644 --- a/cloisim_ros_ground_truth/src/ground_truth.cpp +++ b/cloisim_ros_ground_truth/src/ground_truth.cpp @@ -19,8 +19,9 @@ using namespace std; using namespace cloisim; -using namespace cloisim_ros; +namespace cloisim_ros +{ GroundTruth::GroundTruth(const rclcpp::NodeOptions &options_, const std::string node_name) : Base(node_name, options_) { @@ -100,4 +101,6 @@ void GroundTruth::UpdatePerceptionData() msg_.objects.push_back(object_info_msg); } -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp b/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp index 7cd65cd2..0dab3344 100644 --- a/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp +++ b/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp @@ -13,12 +13,15 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_IMU_HPP_ -#define _CLOISIM_ROS_IMU_HPP_ +#ifndef CLOISIM_ROS_IMU__IMU_HPP_ +#define CLOISIM_ROS_IMU__IMU_HPP_ + +#include + +#include #include #include -#include namespace cloisim_ros { @@ -47,4 +50,4 @@ class Imu : public Base rclcpp::Publisher::SharedPtr pub_; }; } // namespace cloisim_ros -#endif \ No newline at end of file +#endif // CLOISIM_ROS_IMU__IMU_HPP_ diff --git a/cloisim_ros_imu/src/imu.cpp b/cloisim_ros_imu/src/imu.cpp index ffd273c8..4e1ba004 100644 --- a/cloisim_ros_imu/src/imu.cpp +++ b/cloisim_ros_imu/src/imu.cpp @@ -18,7 +18,9 @@ #include using namespace std; -using namespace cloisim_ros; + +namespace cloisim_ros +{ Imu::Imu(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) : Base(node_name, namespace_, options_) @@ -98,4 +100,6 @@ void Imu::PublishData(const string &buffer) fill(begin(msg_imu_.linear_acceleration_covariance), end(msg_imu_.linear_acceleration_covariance), 0.0); pub_->publish(msg_imu_); -} \ No newline at end of file +} + +} // namespace cloisim_ros 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 6cf6e405..e946732d 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 @@ -12,45 +12,49 @@ * * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_JOINTCONTROL_HPP__ -#define _CLOISIM_ROS_JOINTCONTROL_HPP__ +#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 -#include -#include namespace cloisim_ros { - class JointControl : public Base - { - public: - explicit JointControl(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit JointControl(const std::string namespace_ = ""); - virtual ~JointControl(); +class JointControl : public Base +{ + public: + explicit JointControl(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit JointControl(const std::string namespace_ = ""); + virtual ~JointControl(); - private: - void Initialize() override; - void Deinitialize() override { }; + private: + void Initialize() override; + void Deinitialize() override{}; - private: - void PublishData(const std::string &buffer); + private: + void PublishData(const std::string &buffer); - void JointControlWrite(zmq::Bridge* const bridge_ptr, const std::string &buffer); - std::string MakeCommandMessage(const std::string joint_name, const double joint_displacement, const double joint_velocity) const; + void JointControlWrite(zmq::Bridge *const bridge_ptr, const std::string &buffer); + std::string MakeCommandMessage(const std::string joint_name, const double joint_displacement, const double joint_velocity) const; - private: - zmq::Bridge *info_bridge_ptr; + private: + zmq::Bridge *info_bridge_ptr; - std::map target_transform_name; + std::map target_transform_name; - // ROS2 JointControl publisher - rclcpp::Publisher::SharedPtr pub_joint_state_; + // ROS2 JointControl publisher + rclcpp::Publisher::SharedPtr pub_joint_state_; - // ROS2 Joint Jog subscriber - rclcpp::Subscription::SharedPtr sub_joint_job_; - }; -} -#endif + // ROS2 Joint Jog subscriber + rclcpp::Subscription::SharedPtr sub_joint_job_; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_JOINT_CONTROL__JOINT_CONTROL_HPP_ diff --git a/cloisim_ros_joint_control/src/joint_control.cpp b/cloisim_ros_joint_control/src/joint_control.cpp index 5c3524c6..c990927b 100644 --- a/cloisim_ros_joint_control/src/joint_control.cpp +++ b/cloisim_ros_joint_control/src/joint_control.cpp @@ -23,9 +23,11 @@ using namespace std; using namespace chrono_literals; -using namespace placeholders; +using namespace std::placeholders; using namespace cloisim; -using namespace cloisim_ros; + +namespace cloisim_ros +{ JointControl::JointControl(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) : Base(node_name, namespace_, options_) @@ -69,17 +71,20 @@ void JointControl::Initialize() { data_bridge_ptr->Connect(zmq::Bridge::Mode::PUB, portRx, hashKeyPub); data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portTx, hashKeySub); - AddPublisherThread(data_bridge_ptr, bind(&JointControl::PublishData, this, std::placeholders::_1)); + AddPublisherThread(data_bridge_ptr, + 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)); + 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 + auto callback_sub = [this, data_bridge_ptr]( + const control_msgs::msg::JointJog::SharedPtr msg) -> void { // const auto duration = msg->duration; for (size_t i = 0; i < msg->joint_names.size(); i++) @@ -95,13 +100,18 @@ void JointControl::Initialize() }; // ROS2 Publisher - pub_joint_state_ = create_publisher("joint_states", rclcpp::SensorDataQoS()); + pub_joint_state_ = create_publisher( + "joint_states", rclcpp::SensorDataQoS()); // ROS2 Subscriber - sub_joint_job_ = create_subscription("joint_command", rclcpp::SensorDataQoS(), callback_sub); + sub_joint_job_ = create_subscription( + "joint_command", rclcpp::SensorDataQoS(), callback_sub); } -string JointControl::MakeCommandMessage(const string joint_name, const double joint_displacement, const double joint_velocity) const +string JointControl::MakeCommandMessage( + const string joint_name, + const double joint_displacement, + const double joint_velocity) const { msgs::JointCmd jointCmd; @@ -148,4 +158,6 @@ void JointControl::PublishData(const string &buffer) // publish data pub_joint_state_->publish(msg_jointstate); -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_joint_control/src/main.cpp b/cloisim_ros_joint_control/src/main.cpp index 4fc15970..cdf287d3 100644 --- a/cloisim_ros_joint_control/src/main.cpp +++ b/cloisim_ros_joint_control/src/main.cpp @@ -1,4 +1,4 @@ - /** +/** * @file main.cpp * @date 2021-01-14 * @author Hyunseok Yang @@ -51,4 +51,4 @@ int main(int argc, char** argv) executor.spin(); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_lidar/include/cloisim_ros_lidar/lidar.hpp b/cloisim_ros_lidar/include/cloisim_ros_lidar/lidar.hpp index b1e8f82b..e82311b0 100644 --- a/cloisim_ros_lidar/include/cloisim_ros_lidar/lidar.hpp +++ b/cloisim_ros_lidar/include/cloisim_ros_lidar/lidar.hpp @@ -13,49 +13,51 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_LIDAR_HPP_ -#define _CLOISIM_ROS_LIDAR_HPP_ +#ifndef CLOISIM_ROS_LIDAR__LIDAR_HPP_ +#define CLOISIM_ROS_LIDAR__LIDAR_HPP_ + +#include + +#include #include #include #include -#include namespace cloisim_ros { - class Lidar : public Base - { - using LaserScanPub = rclcpp::Publisher::SharedPtr; - using PointCloud2Pub = rclcpp::Publisher::SharedPtr; - - public: - explicit Lidar(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit Lidar(const std::string namespace_ = ""); - ~Lidar(); - - private: - void Initialize() override; - void Deinitialize() override { }; - - private: - std::string GetOutputType(zmq::Bridge* const bridge_ptr); - - void PublishData(const std::string &buffer); - void UpdatePointCloudData(const double min_intensity = 0.0); - void UpdateLaserData(const double min_intensity = 0.0); - - private: - - // buffer from simulation - cloisim::msgs::LaserScanStamped pb_buf_; - - // message for ROS2 communictaion - sensor_msgs::msg::LaserScan msg_laser_; - sensor_msgs::msg::PointCloud2 msg_pc2_; - - // Laser publisher - LaserScanPub pub_laser_; - PointCloud2Pub pub_pc2_; - }; -} -#endif \ No newline at end of file +class Lidar : public Base +{ + using LaserScanPub = rclcpp::Publisher::SharedPtr; + using PointCloud2Pub = rclcpp::Publisher::SharedPtr; + + public: + explicit Lidar(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit Lidar(const std::string namespace_ = ""); + ~Lidar(); + + private: + void Initialize() override; + void Deinitialize() override{}; + + private: + std::string GetOutputType(zmq::Bridge *const bridge_ptr); + + void PublishData(const std::string &buffer); + void UpdatePointCloudData(const double min_intensity = 0.0); + void UpdateLaserData(const double min_intensity = 0.0); + + private: + // buffer from simulation + cloisim::msgs::LaserScanStamped pb_buf_; + + // message for ROS2 communictaion + sensor_msgs::msg::LaserScan msg_laser_; + sensor_msgs::msg::PointCloud2 msg_pc2_; + + // Laser publisher + LaserScanPub pub_laser_; + PointCloud2Pub pub_pc2_; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_LIDAR__LIDAR_HPP_ diff --git a/cloisim_ros_lidar/src/lidar.cpp b/cloisim_ros_lidar/src/lidar.cpp index 63451520..4dcfe049 100644 --- a/cloisim_ros_lidar/src/lidar.cpp +++ b/cloisim_ros_lidar/src/lidar.cpp @@ -12,15 +12,18 @@ * * SPDX-License-Identifier: MIT */ +#include -#include "cloisim_ros_lidar/lidar.hpp" #include + +#include "cloisim_ros_lidar/lidar.hpp" #include -#include using namespace std; using namespace cloisim; -using namespace cloisim_ros; + +namespace cloisim_ros +{ Lidar::Lidar(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) : Base(node_name, namespace_, options_) @@ -264,4 +267,6 @@ void Lidar::UpdateLaserData(const double min_intensity) msg_laser_.intensities.begin(), [min_intensity](double i) -> double { return i > min_intensity ? i : min_intensity; }); -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp index 853fa94b..2644792b 100644 --- a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp +++ b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp @@ -12,15 +12,19 @@ * * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_MICOM_HPP__ -#define _CLOISIM_ROS_MICOM_HPP__ +#ifndef CLOISIM_ROS_MICOM__MICOM_HPP_ +#define CLOISIM_ROS_MICOM__MICOM_HPP_ + +#include + +#include +#include #include #include #include #include #include -#include namespace cloisim_ros { @@ -80,4 +84,4 @@ class Micom : public Base rclcpp::Service::SharedPtr srv_reset_odom_; }; } // namespace cloisim_ros -#endif +#endif // CLOISIM_ROS_MICOM__MICOM_HPP_ diff --git a/cloisim_ros_micom/src/main.cpp b/cloisim_ros_micom/src/main.cpp index f16cada3..f5bb5c92 100644 --- a/cloisim_ros_micom/src/main.cpp +++ b/cloisim_ros_micom/src/main.cpp @@ -1,4 +1,4 @@ - /** +/** * @file main.cpp * @date 2021-01-14 * @author Hyunseok Yang @@ -51,4 +51,4 @@ int main(int argc, char** argv) executor.spin(); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp index f8420e51..736c0374 100644 --- a/cloisim_ros_micom/src/micom.cpp +++ b/cloisim_ros_micom/src/micom.cpp @@ -21,12 +21,17 @@ #include using namespace std; -using namespace chrono_literals; -using namespace placeholders; +using namespace std::chrono_literals; +using namespace std::placeholders; using namespace cloisim; -using namespace cloisim_ros; -Micom::Micom(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) +namespace cloisim_ros +{ + +Micom::Micom( + const rclcpp::NodeOptions &options_, + const string node_name, + const string namespace_) : Base(node_name, namespace_, options_) { Start(); @@ -53,7 +58,9 @@ void Micom::Initialize() const auto hashKeyPub = GetTargetHashKey("Rx"); const auto hashKeySub = GetTargetHashKey("Tx"); const auto hashKeyTf = GetTargetHashKey("Tf"); - DBG_SIM_INFO("hashKey: info(%s) pub(%s) sub(%s) tf(%s)", hashKeyInfo.c_str(), hashKeyPub.c_str(), hashKeySub.c_str(), hashKeyTf.c_str()); + DBG_SIM_INFO("hashKey: info(%s) pub(%s) sub(%s) tf(%s)", + hashKeyInfo.c_str(), hashKeyPub.c_str(), + hashKeySub.c_str(), hashKeyTf.c_str()); { auto base_link_pose = IdentityPose(); @@ -68,7 +75,8 @@ void Micom::Initialize() GetStaticTransforms(info_bridge_ptr); } - pub_battery_ = create_publisher("battery_state", rclcpp::SensorDataQoS()); + pub_battery_ = create_publisher( + "battery_state", rclcpp::SensorDataQoS()); { msg_odom_.header.frame_id = "odom"; @@ -76,13 +84,15 @@ void Micom::Initialize() SetTf2(odom_tf_, msg_odom_.child_frame_id, msg_odom_.header.frame_id); - pub_odom_ = create_publisher("odom", rclcpp::SystemDefaultsQoS()); + pub_odom_ = create_publisher( + "odom", rclcpp::SystemDefaultsQoS()); } { msg_imu_.header.frame_id = "imu_link"; - pub_imu_ = create_publisher("imu", rclcpp::SensorDataQoS()); + pub_imu_ = create_publisher( + "imu", rclcpp::SensorDataQoS()); } auto data_bridge_ptr = CreateBridge(); @@ -107,9 +117,11 @@ void Micom::Initialize() }; // ROS2 Subscriber - sub_micom_ = create_subscription("cmd_vel", rclcpp::SensorDataQoS(), callback_sub); + sub_micom_ = create_subscription( + "cmd_vel", rclcpp::SensorDataQoS(), callback_sub); - srv_reset_odom_ = create_service("reset_odometry", std::bind(&Micom::ResetOdometryCallback, this, _1, _2, _3)); + srv_reset_odom_ = create_service( + "reset_odometry", std::bind(&Micom::ResetOdometryCallback, this, _1, _2, _3)); } void Micom::ResetOdometryCallback( @@ -120,7 +132,8 @@ void Micom::ResetOdometryCallback( RequestReplyMessage(info_bridge_ptr, "reset_odometry"); } -string Micom::MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) const +string Micom::MakeControlMessage( + const geometry_msgs::msg::Twist::SharedPtr msg) const { msgs::Twist twistBuf; // m/s and rad/s auto linear_ptr = twistBuf.mutable_linear(); @@ -223,4 +236,6 @@ void Micom::UpdateBattery() msg_battery_.header.stamp = GetTime(); msg_battery_.voltage = 0.0; msg_battery_.current = 0.0; -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_multicamera/include/cloisim_ros_multicamera/multicamera.hpp b/cloisim_ros_multicamera/include/cloisim_ros_multicamera/multicamera.hpp index 1f06893c..5b272dfd 100644 --- a/cloisim_ros_multicamera/include/cloisim_ros_multicamera/multicamera.hpp +++ b/cloisim_ros_multicamera/include/cloisim_ros_multicamera/multicamera.hpp @@ -11,44 +11,50 @@ * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ -#ifndef _CLOISIM_ROS_MULTICAMERA_HPP_ -#define _CLOISIM_ROS_MULTICAMERA_HPP_ +#ifndef CLOISIM_ROS_MULTICAMERA__MULTICAMERA_HPP_ +#define CLOISIM_ROS_MULTICAMERA__MULTICAMERA_HPP_ +#include +#include + +#include +#include +#include +#include + +#include #include #include -#include #include -#include -#include namespace cloisim_ros { - class MultiCamera : public Base - { - public: - explicit MultiCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit MultiCamera(const std::string namespace_ = ""); - virtual ~MultiCamera(); - - private: - void Initialize() override; - void Deinitialize() override; - - private: - void PublishData(const std::string &buffer); - - private: - // buffer from simulation - cloisim::msgs::ImagesStamped pb_buf_; - - // message for ROS2 communictaion - std::map msg_imgs_; - - // Camera info managers - std::vector> camera_info_manager_; - - // Image publisher - std::vector pubs_; - }; -} -#endif +class MultiCamera : public Base +{ + public: + explicit MultiCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit MultiCamera(const std::string namespace_ = ""); + virtual ~MultiCamera(); + + private: + void Initialize() override; + void Deinitialize() override; + + private: + void PublishData(const std::string &buffer); + + private: + // buffer from simulation + cloisim::msgs::ImagesStamped pb_buf_; + + // message for ROS2 communictaion + std::map msg_imgs_; + + // Camera info managers + std::vector> camera_info_manager_; + + // Image publisher + std::vector pubs_; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_MULTICAMERA__MULTICAMERA_HPP_ diff --git a/cloisim_ros_multicamera/src/multicamera.cpp b/cloisim_ros_multicamera/src/multicamera.cpp index 3fec310b..bb26f0a3 100644 --- a/cloisim_ros_multicamera/src/multicamera.cpp +++ b/cloisim_ros_multicamera/src/multicamera.cpp @@ -11,18 +11,21 @@ * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ +#include +#include + +#include #include "cloisim_ros_multicamera/multicamera.hpp" #include -#include -#include -#include using namespace std; -using namespace chrono_literals; -using namespace cloisim_ros; +using namespace std::chrono_literals; using namespace cloisim; +namespace cloisim_ros +{ + MultiCamera::MultiCamera(const rclcpp::NodeOptions &options_, const string node_name, const std::string namespace_) : Base(node_name, namespace_, options_) { @@ -106,7 +109,7 @@ void MultiCamera::PublishData(const string &buffer) SetTime(pb_buf_.time()); - if (int(camera_info_manager_.size()) != pb_buf_.image_size()) + if (static_cast(camera_info_manager_.size()) != pb_buf_.image_size()) { DBG_SIM_ERR("camera_info_manager is not ready for multi-camera %d != %d", camera_info_manager_.size(), pb_buf_.image_size()); @@ -132,4 +135,6 @@ void MultiCamera::PublishData(const string &buffer) pubs_.at(i).publish(*msg_img, camera_info_msg); } -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_realsense/include/cloisim_ros_realsense/realsense.hpp b/cloisim_ros_realsense/include/cloisim_ros_realsense/realsense.hpp index b195abd6..0d145bde 100644 --- a/cloisim_ros_realsense/include/cloisim_ros_realsense/realsense.hpp +++ b/cloisim_ros_realsense/include/cloisim_ros_realsense/realsense.hpp @@ -12,53 +12,58 @@ * All Rights are Reserved. */ -#ifndef _CLOISIM_ROS_REALSENSE_HPP_ -#define _CLOISIM_ROS_REALSENSE_HPP_ +#ifndef CLOISIM_ROS_REALSENSE__REALSENSE_HPP_ +#define CLOISIM_ROS_REALSENSE__REALSENSE_HPP_ +#include +#include +#include +#include +#include + +#include #include #include -#include #include #include -#include namespace cloisim_ros { - class RealSense : public Base - { - public: - explicit RealSense(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit RealSense(const std::string namespace_ = ""); - virtual ~RealSense(); +class RealSense : public Base +{ + public: + explicit RealSense(const rclcpp::NodeOptions& options_, const std::string node_name, const std::string namespace_ = ""); + explicit RealSense(const std::string namespace_ = ""); + virtual ~RealSense(); - private: - void Initialize() override; - void Deinitialize() override; + private: + void Initialize() override; + void Deinitialize() override; - private: - void InitializeCam(const std::string module_name, zmq::Bridge* const info_ptr, zmq::Bridge* const data_ptr); - void InitializeImu(zmq::Bridge* const info_ptr, zmq::Bridge* const data_ptr); - void PublishImgData(const zmq::Bridge* const bridge_ptr, const std::string &buffer); - void PublishImuData(const std::string &buffer); - void GetActivatedModules(zmq::Bridge *const bridge_ptr); + private: + void InitializeCam(const std::string module_name, zmq::Bridge* const info_ptr, zmq::Bridge* const data_ptr); + void InitializeImu(zmq::Bridge* const info_ptr, zmq::Bridge* const data_ptr); + void PublishImgData(const zmq::Bridge* const bridge_ptr, const std::string& buffer); + void PublishImuData(const std::string& buffer); + void GetActivatedModules(zmq::Bridge* const bridge_ptr); - private: - std::vector> activated_modules_; + private: + std::vector> activated_modules_; - // message for ROS2 communictaion - std::map msg_imgs_; + // message for ROS2 communictaion + std::map msg_imgs_; - // Camera info managers - std::map> camera_info_managers_; + // Camera info managers + std::map> camera_info_managers_; - // Image publisher - std::map pubs_; + // Image publisher + std::map pubs_; - // IMU msgs - sensor_msgs::msg::Imu msg_imu_; + // IMU msgs + sensor_msgs::msg::Imu msg_imu_; - // publisher - rclcpp::Publisher::SharedPtr pub_imu_; - }; -} -#endif \ No newline at end of file + // publisher + rclcpp::Publisher::SharedPtr pub_imu_; +}; +} // namespace cloisim_ros +#endif // CLOISIM_ROS_REALSENSE__REALSENSE_HPP_ diff --git a/cloisim_ros_realsense/src/main.cpp b/cloisim_ros_realsense/src/main.cpp index dd324f49..e831b3c2 100644 --- a/cloisim_ros_realsense/src/main.cpp +++ b/cloisim_ros_realsense/src/main.cpp @@ -8,7 +8,7 @@ * @remark * @warning * LGE Advanced Robotics Laboratory - * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea + * Copyright 2020 LG Electronics Inc. , LTD., Seoul, Korea * All Rights are Reserved. * * SPDX-License-Identifier: MIT diff --git a/cloisim_ros_realsense/src/realsense.cpp b/cloisim_ros_realsense/src/realsense.cpp index 408279c9..006144d9 100644 --- a/cloisim_ros_realsense/src/realsense.cpp +++ b/cloisim_ros_realsense/src/realsense.cpp @@ -12,22 +12,29 @@ * All Rights are Reserved. */ -#include "cloisim_ros_realsense/realsense.hpp" -#include -#include #include #include #include + #include #include #include +#include "cloisim_ros_realsense/realsense.hpp" +#include +#include + using namespace std; using namespace chrono_literals; using namespace cloisim; -using namespace cloisim_ros; -RealSense::RealSense(const rclcpp::NodeOptions& options_, const string node_name, const string namespace_) +namespace cloisim_ros +{ + +RealSense::RealSense( + const rclcpp::NodeOptions& options_, + const string node_name, + const string namespace_) : Base(node_name, namespace_, options_) { Start(); @@ -77,7 +84,8 @@ void RealSense::Initialize() const auto hashKeyData = GetTargetHashKey(module_name + "Data"); const auto hashKeyInfo = GetTargetHashKey(module_name + "Info"); - DBG_SIM_INFO("module: %s, hashKey: data(%s), info(%s)", module_name.c_str(), hashKeyData.c_str(), hashKeyInfo.c_str()); + DBG_SIM_INFO("module: %s, hashKey: data(%s), info(%s)", + module_name.c_str(), hashKeyData.c_str(), hashKeyInfo.c_str()); auto info_bridge_info = CreateBridge(); if (info_bridge_info->Connect(zmq::Bridge::Mode::CLIENT, portInfo, hashKeyInfo)) @@ -101,7 +109,10 @@ void RealSense::Initialize() } } -void RealSense::InitializeCam(const string module_name, zmq::Bridge* const info_ptr, zmq::Bridge* const data_ptr) +void RealSense::InitializeCam( + const string module_name, + zmq::Bridge* const info_ptr, + zmq::Bridge* const data_ptr) { if (info_ptr != nullptr) { @@ -111,7 +122,8 @@ void RealSense::InitializeCam(const string module_name, zmq::Bridge* const info_ transform_pose.set_name(child_frame_id); SetStaticTf2(transform_pose, header_frame_id); - const auto camInfoManager = std::make_shared(GetNode().get()); + const auto camInfoManager = + std::make_shared(GetNode().get()); const auto camSensorMsg = GetCameraSensorMessage(info_ptr); SetCameraInfoInManager(camInfoManager, camSensorMsg, module_name); @@ -137,7 +149,9 @@ void RealSense::InitializeCam(const string module_name, zmq::Bridge* const info_ } } -void RealSense::InitializeImu(zmq::Bridge* const info_ptr, zmq::Bridge* const data_ptr) +void RealSense::InitializeImu( + zmq::Bridge* const info_ptr, + zmq::Bridge* const data_ptr) { // Get frame for message const auto frame_id = GetFrameId("imu_link"); @@ -194,8 +208,12 @@ void RealSense::GetActivatedModules(zmq::Bridge* const bridge_ptr) { const auto type = param.children(0); const auto name = param.children(1); - if (type.has_value() && type.value().type() == msgs::Any_ValueType_STRING && !type.value().string_value().empty() && - name.has_value() && name.value().type() == msgs::Any_ValueType_STRING && !name.value().string_value().empty()) + if (type.has_value() && + type.value().type() == msgs::Any_ValueType_STRING && + !type.value().string_value().empty() && + name.has_value() && + name.value().type() == msgs::Any_ValueType_STRING && + !name.value().string_value().empty()) { const auto tuple_module = make_tuple(type.value().string_value(), name.value().string_value()); activated_modules_.push_back(tuple_module); @@ -209,7 +227,9 @@ void RealSense::GetActivatedModules(zmq::Bridge* const bridge_ptr) DBG_SIM_INFO("activated_modules: %s", moduleListStr.c_str()); } -void RealSense::PublishImgData(const zmq::Bridge* const bridge_ptr, const std::string& buffer) +void RealSense::PublishImgData( + const zmq::Bridge* const bridge_ptr, + const std::string& buffer) { cloisim::msgs::ImageStamped pb_buf_; if (!pb_buf_.ParseFromString(buffer)) @@ -262,4 +282,6 @@ void RealSense::PublishImuData(const string& buffer) fill(begin(msg_imu_.linear_acceleration_covariance), end(msg_imu_.linear_acceleration_covariance), 0.0); pub_imu_->publish(msg_imu_); -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_sonar/include/cloisim_ros_sonar/sonar.hpp b/cloisim_ros_sonar/include/cloisim_ros_sonar/sonar.hpp index 8b063d0a..e64dd559 100644 --- a/cloisim_ros_sonar/include/cloisim_ros_sonar/sonar.hpp +++ b/cloisim_ros_sonar/include/cloisim_ros_sonar/sonar.hpp @@ -13,12 +13,15 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_SONAR_HPP_ -#define _CLOISIM_ROS_SONAR_HPP_ +#ifndef CLOISIM_ROS_SONAR__SONAR_HPP_ +#define CLOISIM_ROS_SONAR__SONAR_HPP_ + +#include + +#include #include #include -#include namespace cloisim_ros { @@ -47,4 +50,4 @@ class Sonar : public Base rclcpp::Publisher::SharedPtr pub_; }; } // namespace cloisim_ros -#endif \ No newline at end of file +#endif // CLOISIM_ROS_SONAR__SONAR_HPP_ diff --git a/cloisim_ros_sonar/src/sonar.cpp b/cloisim_ros_sonar/src/sonar.cpp index f41655a8..08a4ad71 100644 --- a/cloisim_ros_sonar/src/sonar.cpp +++ b/cloisim_ros_sonar/src/sonar.cpp @@ -18,7 +18,9 @@ #include using namespace std; -using namespace cloisim_ros; + +namespace cloisim_ros +{ Sonar::Sonar(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) : Base(node_name, namespace_, options_) @@ -97,7 +99,9 @@ void Sonar::PublishData(const string &buffer) if (pb_buf_.sonar().has_range_max()) msg_range_.max_range = pb_buf_.sonar().range_max(); if (pb_buf_.sonar().has_range()) - msg_range_.range = (float)pb_buf_.sonar().range(); + msg_range_.range = static_cast(pb_buf_.sonar().range()); pub_->publish(msg_range_); -} \ No newline at end of file +} + +} // namespace cloisim_ros diff --git a/cloisim_ros_websocket_service/include/cloisim_ros_websocket_service/websocket_service.hpp b/cloisim_ros_websocket_service/include/cloisim_ros_websocket_service/websocket_service.hpp index c0f57508..6395ee61 100644 --- a/cloisim_ros_websocket_service/include/cloisim_ros_websocket_service/websocket_service.hpp +++ b/cloisim_ros_websocket_service/include/cloisim_ros_websocket_service/websocket_service.hpp @@ -12,11 +12,13 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_WEBSOCKET_SERVICE_HPP_ -#define _CLOISIM_ROS_WEBSOCKET_SERVICE_HPP_ +#ifndef CLOISIM_ROS_WEBSOCKET_SERVICE__WEBSOCKET_SERVICE_HPP_ +#define CLOISIM_ROS_WEBSOCKET_SERVICE__WEBSOCKET_SERVICE_HPP_ #define ASIO_STANDALONE +#include + #include #include @@ -24,24 +26,22 @@ typedef websocketpp::client client; namespace cloisim_ros { -using namespace std; - class WebSocketService { public: WebSocketService(); - WebSocketService(const string service_port); - WebSocketService(const string bridge_ip, const string service_port); + explicit WebSocketService(const std::string service_port); + explicit WebSocketService(const std::string bridge_ip, const std::string service_port); ~WebSocketService(); private: - string base_uri_; + std::string base_uri_; client client_; // Create a client endpoint websocketpp::connection_hdl conn_hdl_; websocketpp::lib::shared_ptr thread_; - string target_filter; - string payload_; + std::string target_filter; + std::string payload_; bool is_reply_received_; bool is_connected_; @@ -58,10 +58,10 @@ class WebSocketService std::string PopPayload(); - void SetTarget(const string target) { target_filter = target; } + void SetTarget(const std::string target) { target_filter = target; } void Run(); }; } // namespace cloisim_ros -#endif \ No newline at end of file +#endif // CLOISIM_ROS_WEBSOCKET_SERVICE__WEBSOCKET_SERVICE_HPP_ diff --git a/cloisim_ros_websocket_service/src/websocket_service.cpp b/cloisim_ros_websocket_service/src/websocket_service.cpp index e0cc3b36..c950074c 100644 --- a/cloisim_ros_websocket_service/src/websocket_service.cpp +++ b/cloisim_ros_websocket_service/src/websocket_service.cpp @@ -14,8 +14,10 @@ #include "cloisim_ros_websocket_service/websocket_service.hpp" -using namespace cloisim_ros; +using namespace std; +namespace cloisim_ros +{ namespace { constexpr auto DEFAULT_WS_SERVICE_PORT = "8080"; @@ -174,4 +176,5 @@ void WebSocketService::Run() this_thread::sleep_for(500ms); } -} \ No newline at end of file +} +} // namespace cloisim_ros diff --git a/cloisim_ros_world/include/cloisim_ros_world/world.hpp b/cloisim_ros_world/include/cloisim_ros_world/world.hpp index d673e2ba..9cd0c02a 100644 --- a/cloisim_ros_world/include/cloisim_ros_world/world.hpp +++ b/cloisim_ros_world/include/cloisim_ros_world/world.hpp @@ -14,12 +14,15 @@ * SPDX-License-Identifier: MIT */ -#ifndef _CLOISIM_ROS_WORLD_HPP_ -#define _CLOISIM_ROS_WORLD_HPP_ +#ifndef CLOISIM_ROS_WORLD__WORLD_HPP_ +#define CLOISIM_ROS_WORLD__WORLD_HPP_ + +#include + +#include #include #include -#include namespace cloisim_ros { @@ -27,7 +30,7 @@ class World : public Base { public: explicit World(const rclcpp::NodeOptions &options_, const std::string node_name); - explicit World(); + World(); ~World(); private: @@ -45,4 +48,4 @@ class World : public Base rclcpp::Publisher::SharedPtr pub_; }; } // namespace cloisim_ros -#endif \ No newline at end of file +#endif // CLOISIM_ROS_WORLD__WORLD_HPP_ diff --git a/cloisim_ros_world/src/world.cpp b/cloisim_ros_world/src/world.cpp index 9c3de37a..f675036c 100644 --- a/cloisim_ros_world/src/world.cpp +++ b/cloisim_ros_world/src/world.cpp @@ -21,8 +21,9 @@ using namespace std; using namespace cloisim; -using namespace cloisim_ros; +namespace cloisim_ros +{ World::World(const rclcpp::NodeOptions &options_, const std::string node_name) : Base(node_name, options_) { @@ -81,4 +82,5 @@ void World::PublishData(const string &buffer) msg_clock_.clock = GetTime(); pub_->publish(msg_clock_); -} \ No newline at end of file +} +} // namespace cloisim_ros From c2dacc9454087a6bf2a8c4a759e66921e04da38d Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 28 Aug 2023 17:48:43 +0900 Subject: [PATCH 7/9] Update README.md for cloisim_ros_joint_control --- cloisim_ros_joint_control/README.md | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/cloisim_ros_joint_control/README.md b/cloisim_ros_joint_control/README.md index 5f375433..94129fc8 100644 --- a/cloisim_ros_joint_control/README.md +++ b/cloisim_ros_joint_control/README.md @@ -21,16 +21,31 @@ ros2 run cloisim_ros_joint_control standalone --ros-args -p target_model:=cloi1 ``` -## Example Usage +## Example Usage for Joint -### state +### State ```shell -ros2 topic pub -1 /FnB/joint_command control_msgs/msg/JointJog '{joint_names: ["FnB_Display::link"], displacements: [-1.5708], velocities: [2]}' +ros2 topic echo /franka/joint_states ``` -### command +### Command + +#### Displacement + +```shell +ros2 topic pub -1 /FnB/joint_command control_msgs/msg/JointJog '{joint_names: ["link_Display_JOINT"], displacements: [-1.5708]}' +``` + +#### Velocity + +```shell +ros2 topic pub -1 /FnB/joint_command control_msgs/msg/JointJog '{joint_names: ["link_Display_JOINT"], velocities: [2.0]}' +``` + +#### Displacement and Velocity + ```shell -ros2 topic pub -1 /FnB/joint_command control_msgs/msg/JointJog '{joint_names: ["FnB_Display::link"], displacements: [-1.5708], velocities: [2]}' +ros2 topic pub -1 /FnB/joint_command control_msgs/msg/JointJog '{joint_names: ["link_Display_JOINT"], displacements: [-1.5708], velocities: [2.0]}' ``` From d2b3bb0a5b66224ebfd9c5047d46d306f8b8d28f Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Wed, 27 Sep 2023 15:35:30 +0900 Subject: [PATCH 8/9] Modify cloisim_ros_joint_control - able to command joint control with only velocity or only displacement separetely --- .../joint_control.hpp | 6 +++- .../src/joint_control.cpp | 28 +++++++++++++------ 2 files changed, 25 insertions(+), 9 deletions(-) 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 e946732d..2f6ef54c 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 @@ -43,7 +43,11 @@ class JointControl : public Base void PublishData(const std::string &buffer); void JointControlWrite(zmq::Bridge *const bridge_ptr, const std::string &buffer); - std::string MakeCommandMessage(const std::string joint_name, const double joint_displacement, const double joint_velocity) const; + + 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; private: zmq::Bridge *info_bridge_ptr; diff --git a/cloisim_ros_joint_control/src/joint_control.cpp b/cloisim_ros_joint_control/src/joint_control.cpp index c990927b..6f59409b 100644 --- a/cloisim_ros_joint_control/src/joint_control.cpp +++ b/cloisim_ros_joint_control/src/joint_control.cpp @@ -87,13 +87,17 @@ void JointControl::Initialize() 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 = msg->displacements[i]; - const auto velocity = msg->velocities[i]; - // DBG_SIM_INFO("%s %f %f", joint_name.c_str(), displacement, velocity); - const auto msgBuf = MakeCommandMessage(joint_name, displacement, velocity); + 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); } @@ -110,6 +114,8 @@ void JointControl::Initialize() string JointControl::MakeCommandMessage( const string joint_name, + const bool use_displacement, + const bool use_velocity, const double joint_displacement, const double joint_velocity) const { @@ -117,11 +123,17 @@ string JointControl::MakeCommandMessage( jointCmd.set_name(joint_name); - auto position = jointCmd.mutable_position(); - position->set_target(joint_displacement); + if (use_displacement) + { + auto position = jointCmd.mutable_position(); + position->set_target(joint_displacement); + } - auto velocity = jointCmd.mutable_velocity(); - velocity->set_target(joint_velocity); + if (use_velocity) + { + auto velocity = jointCmd.mutable_velocity(); + velocity->set_target(joint_velocity); + } string message; jointCmd.SerializeToString(&message); From 93b1aecfd6ee37f62b6d8511344f041dd3dcb719 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Wed, 27 Sep 2023 15:37:17 +0900 Subject: [PATCH 9/9] Update version info in package.xml --- cloisim_ros_actor/package.xml | 2 +- cloisim_ros_base/package.xml | 2 +- cloisim_ros_bridge_zmq/package.xml | 2 +- cloisim_ros_bringup/package.xml | 2 +- cloisim_ros_bringup_param/package.xml | 2 +- cloisim_ros_camera/package.xml | 2 +- cloisim_ros_depthcamera/package.xml | 2 +- cloisim_ros_elevator_system/package.xml | 2 +- cloisim_ros_gps/package.xml | 2 +- cloisim_ros_ground_truth/package.xml | 2 +- cloisim_ros_imu/package.xml | 2 +- cloisim_ros_joint_control/package.xml | 2 +- cloisim_ros_lidar/package.xml | 2 +- cloisim_ros_micom/package.xml | 2 +- cloisim_ros_msgs/package.xml | 2 +- cloisim_ros_multicamera/package.xml | 2 +- cloisim_ros_protobuf_msgs/package.xml | 2 +- cloisim_ros_realsense/package.xml | 2 +- cloisim_ros_sonar/package.xml | 2 +- cloisim_ros_websocket_service/package.xml | 2 +- cloisim_ros_world/package.xml | 2 +- 21 files changed, 21 insertions(+), 21 deletions(-) diff --git a/cloisim_ros_actor/package.xml b/cloisim_ros_actor/package.xml index d9f1ffa4..ad61ee73 100644 --- a/cloisim_ros_actor/package.xml +++ b/cloisim_ros_actor/package.xml @@ -2,7 +2,7 @@ cloisim_ros_actor - 3.3.1 + 3.4.0 node for actor plugin Hyunseok Yang diff --git a/cloisim_ros_base/package.xml b/cloisim_ros_base/package.xml index 681d3e2e..155e65b5 100644 --- a/cloisim_ros_base/package.xml +++ b/cloisim_ros_base/package.xml @@ -2,7 +2,7 @@ cloisim_ros_base - 3.3.1 + 3.4.0 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 d671fa9d..f93f8067 100644 --- a/cloisim_ros_bridge_zmq/package.xml +++ b/cloisim_ros_bridge_zmq/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bridge_zmq - 3.3.1 + 3.4.0 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 1cefbc15..1a0dd202 100644 --- a/cloisim_ros_bringup/package.xml +++ b/cloisim_ros_bringup/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bringup - 3.3.1 + 3.4.0 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 a085491a..eb8403c9 100644 --- a/cloisim_ros_bringup_param/package.xml +++ b/cloisim_ros_bringup_param/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bringup_param - 3.3.1 + 3.4.0 Bringup scripts and configurations for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_camera/package.xml b/cloisim_ros_camera/package.xml index 09c0a58d..10c56e30 100644 --- a/cloisim_ros_camera/package.xml +++ b/cloisim_ros_camera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_camera - 3.3.1 + 3.4.0 virtual camera for cloisim Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_depthcamera/package.xml b/cloisim_ros_depthcamera/package.xml index 51c4ace1..d4fdc004 100644 --- a/cloisim_ros_depthcamera/package.xml +++ b/cloisim_ros_depthcamera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_depthcamera - 3.3.1 + 3.4.0 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 d3bbf4d3..65b4f546 100644 --- a/cloisim_ros_elevator_system/package.xml +++ b/cloisim_ros_elevator_system/package.xml @@ -2,7 +2,7 @@ cloisim_ros_elevator_system - 3.3.1 + 3.4.0 elevator system for simulation Sungkyu Kang diff --git a/cloisim_ros_gps/package.xml b/cloisim_ros_gps/package.xml index 9254bc6d..881f290a 100644 --- a/cloisim_ros_gps/package.xml +++ b/cloisim_ros_gps/package.xml @@ -2,7 +2,7 @@ cloisim_ros_gps - 3.3.1 + 3.4.0 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 52c2e889..af48066d 100644 --- a/cloisim_ros_ground_truth/package.xml +++ b/cloisim_ros_ground_truth/package.xml @@ -2,7 +2,7 @@ cloisim_ros_ground_truth - 3.3.1 + 3.4.0 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 db7889ce..6447e70e 100644 --- a/cloisim_ros_imu/package.xml +++ b/cloisim_ros_imu/package.xml @@ -2,7 +2,7 @@ cloisim_ros_imu - 3.3.1 + 3.4.0 virtual imu for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_joint_control/package.xml b/cloisim_ros_joint_control/package.xml index 10065c3d..261192a9 100644 --- a/cloisim_ros_joint_control/package.xml +++ b/cloisim_ros_joint_control/package.xml @@ -2,7 +2,7 @@ cloisim_ros_joint_control - 3.3.1 + 3.4.0 joint_control package for simulator Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_lidar/package.xml b/cloisim_ros_lidar/package.xml index b7cb721c..90b735b0 100644 --- a/cloisim_ros_lidar/package.xml +++ b/cloisim_ros_lidar/package.xml @@ -2,7 +2,7 @@ cloisim_ros_lidar - 3.3.1 + 3.4.0 virtual lidar for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_micom/package.xml b/cloisim_ros_micom/package.xml index 854a7155..d6c8acf0 100644 --- a/cloisim_ros_micom/package.xml +++ b/cloisim_ros_micom/package.xml @@ -2,7 +2,7 @@ cloisim_ros_micom - 3.3.1 + 3.4.0 micom package for simulator Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_msgs/package.xml b/cloisim_ros_msgs/package.xml index 11837534..4c4bfcd2 100644 --- a/cloisim_ros_msgs/package.xml +++ b/cloisim_ros_msgs/package.xml @@ -2,7 +2,7 @@ cloisim_ros_msgs - 3.3.1 + 3.4.0 interfaces package for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_multicamera/package.xml b/cloisim_ros_multicamera/package.xml index 5c5e19de..cb3356e7 100644 --- a/cloisim_ros_multicamera/package.xml +++ b/cloisim_ros_multicamera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_multicamera - 3.3.1 + 3.4.0 virtual multi-camera for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_protobuf_msgs/package.xml b/cloisim_ros_protobuf_msgs/package.xml index ce487c07..0214dd1f 100644 --- a/cloisim_ros_protobuf_msgs/package.xml +++ b/cloisim_ros_protobuf_msgs/package.xml @@ -2,7 +2,7 @@ cloisim_ros_protobuf_msgs - 3.3.1 + 3.4.0 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 1b09959d..b7945d80 100644 --- a/cloisim_ros_realsense/package.xml +++ b/cloisim_ros_realsense/package.xml @@ -2,7 +2,7 @@ cloisim_ros_realsense - 3.3.1 + 3.4.0 virtual realsense for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_sonar/package.xml b/cloisim_ros_sonar/package.xml index 31aef515..9536e743 100644 --- a/cloisim_ros_sonar/package.xml +++ b/cloisim_ros_sonar/package.xml @@ -2,7 +2,7 @@ cloisim_ros_sonar - 3.3.1 + 3.4.0 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 1ef61188..4223a17c 100644 --- a/cloisim_ros_websocket_service/package.xml +++ b/cloisim_ros_websocket_service/package.xml @@ -2,7 +2,7 @@ cloisim_ros_websocket_service - 3.3.1 + 3.4.0 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 fc518915..814c3e2d 100644 --- a/cloisim_ros_world/package.xml +++ b/cloisim_ros_world/package.xml @@ -2,7 +2,7 @@ cloisim_ros_world - 3.3.1 + 3.4.0 Utilities to interface with Unity through ROS. Hyunseok Yang Hyunseok Yang