diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..419e1b60 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "cloi_common_interfaces"] + path = cloi_common_interfaces + url = https://github.com/lge-ros2/cloi_common_interfaces.git + branch = 4.0.0 \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index 6cfbf74e..cba129cc 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,16 +1,20 @@ FROM ros:humble-ros-base ENV HOSTNAME cloisim_ros +ENV RMW_IMPLEMENTATION rmw_cyclonedds_cpp +ENV DEBIAN_FRONTEND noninteractive +ENV DEBCONF_NOWARNINGS yes WORKDIR /opt/lge-ros2/src -RUN git clone https://github.com/lge-ros2/cloisim_ros.git -b ${ROS_DISTRO} -RUN git clone https://github.com/lge-ros2/cloi_common_interfaces.git -b ${ROS_DISTRO} +RUN git clone --recursive https://github.com/lge-ros2/cloisim_ros.git -b ${ROS_DISTRO} WORKDIR /opt/lge-ros2 -RUN apt update && apt upgrade -y && rosdep update && \ -rosdep install -y -r -q --from-paths src --ignore-src --rosdistro $ROS_DISTRO +RUN apt update && apt upgrade -y && \ + apt install -y -q ros-${ROS_DISTRO}-rmw-cyclonedds-cpp && \ + rosdep update && \ + rosdep install -y -r -q --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} RUN ["/bin/bash", "-c", "source /opt/ros/${ROS_DISTRO}/setup.bash; colcon build --symlink-install"] diff --git a/README.md b/README.md index 88e323cf..92056d9c 100644 --- a/README.md +++ b/README.md @@ -9,15 +9,20 @@ ROS2 simulation device packages to connect CLOiSim(the unity3D based multi-robot ## Install ROS2 humble - follow the guideline on below link. +follow the guideline on below link. ## Prerequisite ```shell +mkdir -p ~/cloisim_ws/src +cd ~/cloisim_ws/src +git clone --recursive https://github.com/lge-ros2/cloisim_ros.git -b humble + sudo apt update sudo apt install -y python3-rosdep + sudo rosdep init rosdep update rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble @@ -29,6 +34,7 @@ Set up ROS2 environment first ```shell source /opt/ros2/humble/setup.bash +cd ~/cloisim_ws colcon build --symlink-install --packages-up-to cloisim_ros_bringup ``` @@ -45,6 +51,14 @@ check here [details](https://github.com/lge-ros2/cloisim_ros/tree/humble/cloisim ### Run cloisim_ros (robot + world) +#### DDS + +recommended to use cylcone DDS + +```shell +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +``` + #### Shared Memory DDS with FastRTPS It's experimental feature for cloisim_ros. Use absolute path in FASTRTPS_DEFAULT_PROFILES_FILE environment variable. diff --git a/cloi_common_interfaces b/cloi_common_interfaces new file mode 160000 index 00000000..c081a262 --- /dev/null +++ b/cloi_common_interfaces @@ -0,0 +1 @@ +Subproject commit c081a262fccd31b109a2bf07ebce16175ade0fbe diff --git a/cloisim_ros_actor/src/actor.cpp b/cloisim_ros_actor/src/actor.cpp index d37ee509..f331f1fb 100644 --- a/cloisim_ros_actor/src/actor.cpp +++ b/cloisim_ros_actor/src/actor.cpp @@ -15,9 +15,10 @@ #include "cloisim_ros_actor/actor.hpp" -using namespace std; using namespace std::placeholders; -using namespace cloisim; +using Param = cloisim::msgs::Param; +using Any = cloisim::msgs::Any; +using string = std::string; namespace cloisim_ros { @@ -59,9 +60,9 @@ void Actor::Initialize() } void Actor::CallMoveActor( - const shared_ptr /*request_header*/, - const shared_ptr request, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) { const auto message = CreateMoveRequest(request->target_name, request->destination); const auto reply = RequestReplyMessage(control_bridge_ptr, message); @@ -71,7 +72,7 @@ void Actor::CallMoveActor( } } -bool Actor::GetResultFromResponse(const msgs::Param &response_msg) +bool Actor::GetResultFromResponse(const Param &response_msg) { if (!response_msg.IsInitialized() || response_msg.name().compare("result") != 0) { @@ -80,15 +81,15 @@ bool Actor::GetResultFromResponse(const msgs::Param &response_msg) return response_msg.value().bool_value(); } -msgs::Param Actor::CreateMoveRequest( +Param Actor::CreateMoveRequest( const string target_name, const geometry_msgs::msg::Vector3 point) { - msgs::Param request_msg; + Param request_msg; request_msg.set_name(target_name); auto value_ptr = request_msg.mutable_value(); - value_ptr->set_type(msgs::Any::VECTOR3D); + value_ptr->set_type(Any::VECTOR3D); auto vector3d_value_ptr = value_ptr->mutable_vector3d_value(); vector3d_value_ptr->set_x(point.x); diff --git a/cloisim_ros_base/include/cloisim_ros_base/camera_helper.h b/cloisim_ros_base/include/cloisim_ros_base/camera_helper.hpp similarity index 96% rename from cloisim_ros_base/include/cloisim_ros_base/camera_helper.h rename to cloisim_ros_base/include/cloisim_ros_base/camera_helper.hpp index c8d5c782..7d8f6a49 100644 --- a/cloisim_ros_base/include/cloisim_ros_base/camera_helper.h +++ b/cloisim_ros_base/include/cloisim_ros_base/camera_helper.hpp @@ -1,5 +1,5 @@ /** - * @file camera_helper.h + * @file camera_helper.hpp * @date 2021-05-16 * @author Hyunseok Yang * @brief @@ -13,8 +13,8 @@ * SPDX-License-Identifier: MIT */ -#ifndef CLOISIM_ROS_BASE__CAMERA_HELPER_H_ -#define CLOISIM_ROS_BASE__CAMERA_HELPER_H_ +#ifndef CLOISIM_ROS_BASE__CAMERA_HELPER_HPP_ +#define CLOISIM_ROS_BASE__CAMERA_HELPER_HPP_ #include #include @@ -178,4 +178,4 @@ static cloisim::msgs::CameraSensor GetCameraSensorMessage( return cameraSensorInfo; } -#endif // CLOISIM_ROS_BASE__CAMERA_HELPER_H_ +#endif // CLOISIM_ROS_BASE__CAMERA_HELPER_HPP_ diff --git a/cloisim_ros_base/include/cloisim_ros_base/helper.h b/cloisim_ros_base/include/cloisim_ros_base/helper.hpp similarity index 94% rename from cloisim_ros_base/include/cloisim_ros_base/helper.h rename to cloisim_ros_base/include/cloisim_ros_base/helper.hpp index 79d4c626..26a64f7d 100644 --- a/cloisim_ros_base/include/cloisim_ros_base/helper.h +++ b/cloisim_ros_base/include/cloisim_ros_base/helper.hpp @@ -1,5 +1,5 @@ /** - * @file helper.h + * @file helper.hpp * @date 2021-01-14 * @author Hyunseok Yang * @brief @@ -13,8 +13,8 @@ * SPDX-License-Identifier: MIT */ -#ifndef CLOISIM_ROS_BASE__HELPER_H_ -#define CLOISIM_ROS_BASE__HELPER_H_ +#ifndef CLOISIM_ROS_BASE__HELPER_HPP_ +#define CLOISIM_ROS_BASE__HELPER_HPP_ #include @@ -114,4 +114,4 @@ static void ConvertPointToVector3( dst.z = src.z; } -#endif // CLOISIM_ROS_BASE__HELPER_H_ +#endif // CLOISIM_ROS_BASE__HELPER_HPP_ diff --git a/cloisim_ros_base/src/base.cpp b/cloisim_ros_base/src/base.cpp index 34c199ca..5528d441 100644 --- a/cloisim_ros_base/src/base.cpp +++ b/cloisim_ros_base/src/base.cpp @@ -16,10 +16,10 @@ #include #include "cloisim_ros_base/base.hpp" -#include "cloisim_ros_base/helper.h" +#include "cloisim_ros_base/helper.hpp" -using namespace std; -using namespace cloisim; +using namespace std::literals::chrono_literals; +using string = std::string; namespace cloisim_ros { @@ -45,16 +45,16 @@ 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(std::vector{"--ros-args", + "--remap", "/tf:=tf", + "--remap", "/tf_static:=tf_static"})) , m_bRunThread(false) - , m_node_handle(shared_ptr(this, [](auto) {})) + , m_node_handle(std::shared_ptr(this, [](auto) {})) , m_static_tf_broadcaster(nullptr) , m_tf_broadcaster(nullptr) , enable_tf_publish_(true) { - get_parameter_or("enable_tf", enable_tf_publish_, bool(true)); + get_parameter_or("enable_tf", enable_tf_publish_, true); } Base::~Base() @@ -117,7 +117,9 @@ void Base::GenerateTF(const string& buffer) newTf.header.stamp = Convert(pb_transform_stamped.header().stamp()); 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()); + // 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(), @@ -161,7 +163,7 @@ void Base::CloseBridges() void Base::AddPublisherThread( zmq::Bridge* const bridge_ptr, - function thread_func) + std::function thread_func) { m_threads.emplace_back( [this, bridge_ptr, thread_func]() @@ -173,7 +175,8 @@ void Base::AddPublisherThread( const bool succeeded = GetBufferFromSimulator(bridge_ptr, &buffer_ptr, bufferLength); if (!succeeded || bufferLength < 0) { - DBG_ERR("[%s] Failed to get buffer(%d) <= Sim, %s", get_name(), bufferLength, zmq_strerror(zmq_errno())); + DBG_ERR("[%s] Failed to get buffer(%d) <= Sim, %s", + get_name(), bufferLength, zmq_strerror(zmq_errno())); continue; } @@ -221,7 +224,7 @@ 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 && + const auto parent_frame_id = (link.value().type() == cloisim::msgs::Any_ValueType_STRING && link.name().compare("parent_frame_id") == 0) ? link.value().string_value() : "base_link"; @@ -233,7 +236,7 @@ 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) + child.value().type() == cloisim::msgs::Any_ValueType_POSE3D) { pose = child.value().pose3d_value(); } @@ -247,12 +250,12 @@ void Base::GetStaticTransforms(zmq::Bridge* const bridge_ptr) } } -msgs::Pose Base::GetObjectTransform( +cloisim::msgs::Pose Base::GetObjectTransform( zmq::Bridge* const bridge_ptr, const string target_name, string& parent_frame_id) { - msgs::Pose transform; + cloisim::msgs::Pose transform; transform.Clear(); if (bridge_ptr == nullptr) @@ -272,7 +275,7 @@ msgs::Pose Base::GetObjectTransform( { const auto child_param = reply.children(0); if (child_param.name() == "parent_frame_id" && child_param.has_value() && - child_param.value().type() == msgs::Any_ValueType_STRING && + child_param.value().type() == cloisim::msgs::Any_ValueType_STRING && !child_param.value().string_value().empty()) { // set parent_frame_id into name if exists. @@ -303,7 +306,7 @@ void Base::GetRos2Parameter(zmq::Bridge* const bridge_ptr) { const auto param = reply.children(i); const auto paramValue = (param.has_value() && - param.value().type() == msgs::Any_ValueType_STRING) + param.value().type() == cloisim::msgs::Any_ValueType_STRING) ? param.value().string_value() : ""; @@ -347,7 +350,7 @@ bool Base::GetBufferFromSimulator( void Base::SetTf2( geometry_msgs::msg::TransformStamped& target_msg, - const msgs::Pose transform, + const cloisim::msgs::Pose transform, const string child_frame_id, const string header_frame_id) { @@ -358,7 +361,7 @@ void Base::SetTf2( } void Base::SetStaticTf2( - const msgs::Pose transform, + const cloisim::msgs::Pose transform, const string parent_header_frame_id) { geometry_msgs::msg::TransformStamped static_tf; @@ -370,12 +373,12 @@ void Base::SetStaticTf2( AddStaticTf2(static_tf); } -msgs::Param Base::RequestReplyMessage( +cloisim::msgs::Param Base::RequestReplyMessage( zmq::Bridge* const bridge_ptr, const string request_message, const string request_value) { - msgs::Param reply; + cloisim::msgs::Param reply; if (bridge_ptr == nullptr) { @@ -384,7 +387,7 @@ msgs::Param Base::RequestReplyMessage( } string serialized_request_data; - msgs::Param request; + cloisim::msgs::Param request; request.set_name(request_message); if (!request_value.empty()) @@ -401,7 +404,10 @@ msgs::Param Base::RequestReplyMessage( if (serialized_reply_data.size() > 0) { if (reply.ParseFromString(serialized_reply_data) == false) - DBG_SIM_ERR("Failed to parse serialized buffer, buffer_ptr(%p) length(%ld)", serialized_reply_data.data(), serialized_reply_data.size()); + { + DBG_SIM_ERR("Failed to parse serialized buffer, buffer_ptr(%p) length(%ld)", + serialized_reply_data.data(), serialized_reply_data.size()); + } } else DBG_SIM_ERR("Failed to get reply data, length(%ld)", serialized_reply_data.size()); @@ -409,11 +415,11 @@ msgs::Param Base::RequestReplyMessage( return reply; } -msgs::Param Base::RequestReplyMessage( +cloisim::msgs::Param Base::RequestReplyMessage( zmq::Bridge* const bridge_ptr, - const msgs::Param request_message) + const cloisim::msgs::Param request_message) { - msgs::Param response_msg; + cloisim::msgs::Param response_msg; string serializedBuffer; request_message.SerializeToString(&serializedBuffer); @@ -427,9 +433,9 @@ msgs::Param Base::RequestReplyMessage( return response_msg; } -msgs::Pose Base::IdentityPose() +cloisim::msgs::Pose Base::IdentityPose() { - msgs::Pose identityTransform; + cloisim::msgs::Pose identityTransform; identityTransform.mutable_position()->set_x(0.0); identityTransform.mutable_position()->set_y(0.0); identityTransform.mutable_position()->set_z(0.0); 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 5813b8b2..009566da 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 @@ -17,10 +17,9 @@ #define CLOISIM_ROS_BRIDGE_ZMQ__BRIDGE_HPP_ #include - -#include "log.h" - #include +#include "cloisim_ros_bridge_zmq/log.h" + namespace cloisim_ros { @@ -85,7 +84,7 @@ class Bridge private: bool Setup(const unsigned char mode); - bool SetupCommon(void* const targetSocket); + bool SetupCommon(void* const socket); bool SetupSubscriber(); bool SetupPublisher(); bool SetupService(); 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 2bf43eb0..a6ac4bc2 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 @@ -15,7 +15,7 @@ #ifndef CLOISIM_ROS_BRIDGE_ZMQ__LOG_H_ #define CLOISIM_ROS_BRIDGE_ZMQ__LOG_H_ -#include "term_color.h" +#include "cloisim_ros_bridge_zmq/term_color.h" #pragma GCC system_header diff --git a/cloisim_ros_bridge_zmq/src/bridge.cpp b/cloisim_ros_bridge_zmq/src/bridge.cpp index 88c4da3e..bc3a5c63 100644 --- a/cloisim_ros_bridge_zmq/src/bridge.cpp +++ b/cloisim_ros_bridge_zmq/src/bridge.cpp @@ -17,10 +17,10 @@ #include #include -using namespace std; - #define DEFAULT_CLOISIM_BRIDGE_IP "127.0.0.1" +using string = std::string; + namespace cloisim_ros::zmq { @@ -82,27 +82,27 @@ bool Bridge::Setup(const unsigned char mode) return result; } -bool Bridge::SetupCommon(void *const targetSocket) +bool Bridge::SetupCommon(void *const socket) { - if (zmq_setsockopt(targetSocket, ZMQ_CONNECT_TIMEOUT, &connect_timeout, sizeof(connect_timeout))) + if (zmq_setsockopt(socket, ZMQ_CONNECT_TIMEOUT, &connect_timeout, sizeof(connect_timeout))) { lastErrMsg = "SetSock Err:" + string(zmq_strerror(zmq_errno())); return false; } - if (zmq_setsockopt(targetSocket, ZMQ_RECONNECT_IVL, &reconnect_ivl_min, sizeof(reconnect_ivl_min))) + if (zmq_setsockopt(socket, ZMQ_RECONNECT_IVL, &reconnect_ivl_min, sizeof(reconnect_ivl_min))) { lastErrMsg = "SetSock Err:" + string(zmq_strerror(zmq_errno())); return false; } - if (zmq_setsockopt(targetSocket, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl_max, sizeof(reconnect_ivl_max))) + if (zmq_setsockopt(socket, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl_max, sizeof(reconnect_ivl_max))) { lastErrMsg = "SetSock Err:" + string(zmq_strerror(zmq_errno())); return false; } - if (zmq_setsockopt(targetSocket, ZMQ_LINGER, &lingerPeriod, sizeof(lingerPeriod))) + if (zmq_setsockopt(socket, ZMQ_LINGER, &lingerPeriod, sizeof(lingerPeriod))) { lastErrMsg = "SetSock Err:" + string(zmq_strerror(zmq_errno())); return false; @@ -313,7 +313,8 @@ bool Bridge::ConnectPublisher(const uint16_t port, const string hashKey) m_nHashTagTx = GetHashCode(hashKey); const auto bridgeAddress = GetAddress(port); - // DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", (void *)this, bridgeAddress.c_str(), m_nHashTagTx); + // DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", + // (void *)this, bridgeAddress.c_str(), m_nHashTagTx); DBG_SIM_MSG("address(%s) hash(%lX)", bridgeAddress.c_str(), m_nHashTagTx); if (zmq_connect(pPub_, bridgeAddress.c_str()) < 0) @@ -330,7 +331,8 @@ bool Bridge::ConnectService(const uint16_t port, const string hashKey) m_nHashTagTx = GetHashCode(hashKey); const auto bridgeAddress = GetAddress(port); - // DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", (void *)this, bridgeAddress.c_str(), m_nHashTagTx); + // DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", + // (void *)this, bridgeAddress.c_str(), m_nHashTagTx); DBG_SIM_MSG("address(%s) hash(%lX)", bridgeAddress.c_str(), m_nHashTagTx); if (zmq_connect(pRep_, bridgeAddress.c_str()) < 0) @@ -347,7 +349,8 @@ bool Bridge::ConnectClient(const uint16_t port, const string hashKey) m_nHashTagTx = GetHashCode(hashKey); const auto bridgeAddress = GetAddress(port); - // DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", (void *)this, bridgeAddress.c_str(), m_nHashTagTx); + // DBG_SIM_MSG("ptr(%p) address(%s) hash(%lX)", + // (void *)this, bridgeAddress.c_str(), m_nHashTagTx); DBG_SIM_MSG("address(%s) hash(%lX)", bridgeAddress.c_str(), m_nHashTagTx); if (zmq_connect(pReq_, bridgeAddress.c_str()) < 0) @@ -404,15 +407,16 @@ bool Bridge::CloseSocket(void *&target) bool Bridge::Receive(void **buffer, int &bufferLength, bool isNonBlockingMode) { - if (&m_msgRx == nullptr || pSockRx_ == nullptr) + if (pSockRx_ == nullptr) { - DBG_SIM_ERR("Cannot Receive data due to uninitialized pointer m_msgRx(%p) or pSockRx_(%p)", (void *)&m_msgRx, pSockRx_); + DBG_SIM_ERR("Cannot Receive data due to uninitialized pointer pSockRx_(%p)", pSockRx_); return false; } if ((bufferLength = zmq_msg_recv(&m_msgRx, pSockRx_, (isNonBlockingMode) ? ZMQ_DONTWAIT : 0)) < 0) { - // DBG_SIM_ERR("Failed to receive message len(%d): %s", bufferLength, zmq_strerror(zmq_errno())); + // DBG_SIM_ERR("Failed to receive message len(%d): %s", + // bufferLength, zmq_strerror(zmq_errno())); return false; } @@ -432,13 +436,15 @@ bool Bridge::Send(const void *buffer, const int bufferLength, bool isNonBlocking zmq_msg_t msg; if (pSockTx_ == nullptr || zmq_msg_init_size(&msg, tagSize + bufferLength) < 0) { - DBG_SIM_ERR("Cannot Send data due to uninitialized pointer, msg(%p) or pSockTx_(%p)", (void *)&msg, pSockTx_); + DBG_SIM_ERR("Cannot Send data due to uninitialized pointer, msg(%p) or pSockTx_(%p)", + (void *)&msg, pSockTx_); return false; } // Set hash Tag memcpy(zmq_msg_data(&msg), &m_nHashTagTx, tagSize); - memcpy(reinterpret_cast(reinterpret_cast(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) diff --git a/cloisim_ros_bringup/CMakeLists.txt b/cloisim_ros_bringup/CMakeLists.txt index 39420773..b499ebe8 100644 --- a/cloisim_ros_bringup/CMakeLists.txt +++ b/cloisim_ros_bringup/CMakeLists.txt @@ -61,6 +61,12 @@ add_executable( src/bringup.cpp ) +target_include_directories( + ${STANDALONE_EXEC_NAME} PRIVATE + $ + $ +) + ament_target_dependencies( ${STANDALONE_EXEC_NAME} ${dependencies} diff --git a/cloisim_ros_bringup/README.md b/cloisim_ros_bringup/README.md index 07b818ef..3d825929 100644 --- a/cloisim_ros_bringup/README.md +++ b/cloisim_ros_bringup/README.md @@ -6,7 +6,7 @@ The `cloisim_ros_bringup` package is an bringup system for cloisim_ros. ```shell ros2 launch cloisim_ros_bringup cloisim.launch.py -ros2 launch cloisim_ros_bringup bringup.launch.py +ros2 launch cloisim_ros_bringup bringup_launch.py ``` ## ROS2 Run @@ -31,8 +31,9 @@ ros2 run cloisim_ros_bringup bringup #### ros2 launch ```shell -ros2 launch cloisim_ros_bringup bringup.launch.py -ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=False +ros2 launch cloisim_ros_bringup bringup_launch.py +ros2 launch cloisim_ros_bringup bringup_launch.py single_mode:=False +ros2 launch cloisim_ros_bringup bringup_multi_launch.py ``` ### Specify the target model or target parts @@ -42,10 +43,10 @@ Available Parts(case sensitive): MICOM, LASER, LIDAR, CAMERA, DEPTHCAMERA, MULTI Examples ```shell -ros2 launch cloisim_ros_bringup bringup.launch.py target_model:=cloi1 -ros2 launch cloisim_ros_bringup bringup.launch.py target_parts_type:=LASER -ros2 launch cloisim_ros_bringup bringup.launch.py target_model:=cloi1 target_parts_type:=LASER -ros2 launch cloisim_ros_bringup bringup.launch.py target_model:=cloi1 target_parts_type:=LASER target_parts_name:=lidar +ros2 launch cloisim_ros_bringup bringup_launch.py target_model:=cloi1 +ros2 launch cloisim_ros_bringup bringup_launch.py target_parts_type:=LASER +ros2 launch cloisim_ros_bringup bringup_launch.py target_model:=cloi1 target_parts_type:=LASER +ros2 launch cloisim_ros_bringup bringup_launch.py target_model:=cloi1 target_parts_type:=LASER target_parts_name:=lidar ``` ### Turn on single Mode @@ -54,7 +55,8 @@ will NOT apply namespace for robot and the number of robot must BE single in wor ```shell ros2 run cloisim_ros_bringup bringup --ros-args -p single_mode:=True -ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True +ros2 launch cloisim_ros_bringup bringup_launch.py single_mode:=True +ros2 launch cloisim_ros_bringup bringup_single_launch.py ``` #### Specific target model without namespace @@ -62,7 +64,8 @@ ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True Specify target model from simulation ```shell -ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True target_model:=cloi0 +ros2 launch cloisim_ros_bringup bringup_launch.py single_mode:=True target_model:=cloi0 +ros2 launch cloisim_ros_bringup bringup_single_launch.py target_model:=cloi0 ``` ### Enable or disable TF/TF_Static publishing @@ -70,6 +73,7 @@ ros2 launch cloisim_ros_bringup bringup.launch.py single_mode:=True target_model Currently only support "micom" type in cloisim_ros_bringup. ```shell -ros2 launch cloisim_ros_bringup bringup.launch.py micom.enable_tf:=False ros2 run cloisim_ros_bringup bringup --ros-args -p single_mode:=True -p micom.enable_tf:=False +ros2 launch cloisim_ros_bringup bringup_launch.py single_mode:=True micom.enable_tf:=False +ros2 launch cloisim_ros_bringup bringup_single_launch.py micom.enable_tf:=False ``` diff --git a/cloisim_ros_bringup/include/cloisim_ros_bringup/type.h b/cloisim_ros_bringup/include/cloisim_ros_bringup/type.h new file mode 100644 index 00000000..e4f0bc07 --- /dev/null +++ b/cloisim_ros_bringup/include/cloisim_ros_bringup/type.h @@ -0,0 +1,32 @@ +/** + * @file type.h + * @date 2024-07-22 + * @author Hyunseok Yang + * @brief + * @remark + * @copyright + * LGE Advanced Robotics Laboratory + * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea + * All Rights are Reserved. + * + * SPDX-License-Identifier: MIT + */ + +#ifndef CLOISIM_ROS_BRINGUP__TYPE_H_ +#define CLOISIM_ROS_BRINGUP__TYPE_H_ + +#include +#include +#include +#include +#include + +// , +typedef std::map, + std::tuple>> + node_map_t; + +// +typedef std::vector> loaded_key_list_t; + +#endif // CLOISIM_ROS_BRINGUP__TYPE_H_ diff --git a/cloisim_ros_bringup/launch/bringup_multi_launch.py b/cloisim_ros_bringup/launch/bringup_multi_launch.py new file mode 100644 index 00000000..7658e415 --- /dev/null +++ b/cloisim_ros_bringup/launch/bringup_multi_launch.py @@ -0,0 +1,29 @@ +""" +LGE Advanced Robotics Laboratory +Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea +All Rights are Reserved. + +SPDX-License-Identifier: MIT +""" + +import launch.actions +from ament_index_python.packages import get_package_share_directory +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + +def generate_launch_description(): + + bringup_dir = get_package_share_directory('cloisim_ros_bringup') + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([bringup_dir, "launch", "bringup_launch.py"]) + ), + launch_arguments={'single_mode': False}.items() + ) + + ld = launch.LaunchDescription() + ld.add_action(bringup_cmd) + + return ld diff --git a/cloisim_ros_bringup/launch/bringup_single_launch.py b/cloisim_ros_bringup/launch/bringup_single_launch.py new file mode 100644 index 00000000..c8365cfb --- /dev/null +++ b/cloisim_ros_bringup/launch/bringup_single_launch.py @@ -0,0 +1,29 @@ +""" +LGE Advanced Robotics Laboratory +Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea +All Rights are Reserved. + +SPDX-License-Identifier: MIT +""" + +import launch.actions +from ament_index_python.packages import get_package_share_directory +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + +def generate_launch_description(): + + bringup_dir = get_package_share_directory('cloisim_ros_bringup') + + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([bringup_dir, "launch", "bringup_launch.py"]) + ), + launch_arguments={'single_mode': True}.items() + ) + + ld = launch.LaunchDescription() + ld.add_action(bringup_cmd) + + return ld diff --git a/cloisim_ros_bringup/src/bringup.cpp b/cloisim_ros_bringup/src/bringup.cpp index 28021cc6..9914874f 100644 --- a/cloisim_ros_bringup/src/bringup.cpp +++ b/cloisim_ros_bringup/src/bringup.cpp @@ -4,7 +4,7 @@ * @author Hyunseok Yang * @brief * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea * All Rights are Reserved. @@ -28,23 +28,28 @@ #include #include #include +#include "cloisim_ros_bringup/type.h" -using namespace std; +using namespace std::literals::chrono_literals; +using string = std::string; +using std::cout; +using std::endl; // , -map, tuple>> g_node_map_list; +node_map_t g_node_map_list; + // -static vector> loaded_key_list; +static loaded_key_list_t loaded_key_list; static bool g_enable_single_mode = false; -static shared_ptr make_device_node( +static std::shared_ptr make_device_node( rclcpp::NodeOptions& node_options, const string& node_type, const string& model_name, const string& node_name, const Json::Value& node_param) { - shared_ptr node = nullptr; + std::shared_ptr node = nullptr; if (g_enable_single_mode) node_options.append_parameter_override("single_mode.robotname", model_name); @@ -136,13 +141,13 @@ static shared_ptr make_device_node( return node; } -static shared_ptr make_world_node( +static std::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; + std::shared_ptr node = nullptr; node_options.append_parameter_override("model", model_name); @@ -164,7 +169,7 @@ static void parse_target_parts_by_name( const string model_name, const string node_name) { - shared_ptr node = nullptr; + std::shared_ptr node = nullptr; rclcpp::NodeOptions node_options; node_options.append_parameter_override("single_mode", static_cast(g_enable_single_mode)); @@ -173,7 +178,8 @@ static void parse_target_parts_by_name( const auto key = tie(model_name, node_type, node_name); loaded_key_list.push_back(key); - // const auto node_info = "Target Model/Type/Parts: " + model_name + "/" + node_type + "/" + node_name; + // const auto node_info = "Target Model/Type/Parts: " + + // model_name + "/" + node_type + "/" + node_name; // cout << "node info: " << node_info << endl; if (g_node_map_list.find(key) != g_node_map_list.end()) { @@ -260,7 +266,7 @@ void make_bringup_list( else { parse_target_model(item_list, item_name, target_parts_type, target_parts_name); - this_thread::sleep_for(100ms); + std::this_thread::sleep_for(100ms); } } @@ -279,8 +285,8 @@ void make_bringup_list( if (find(loaded_key_list.begin(), loaded_key_list.end(), key) == loaded_key_list.end()) { auto& value = it->second; - if (get<1>(value) == false) - get<1>(value) = true; // mark to remove + if (std::get<1>(value) == false) + std::get<1>(value) = true; // mark to remove } } } diff --git a/cloisim_ros_bringup/src/main.cpp b/cloisim_ros_bringup/src/main.cpp index 7929854a..a2c3fb27 100644 --- a/cloisim_ros_bringup/src/main.cpp +++ b/cloisim_ros_bringup/src/main.cpp @@ -4,7 +4,7 @@ * @author Hyunseok Yang * @brief * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea * All Rights are Reserved. @@ -13,15 +13,18 @@ */ #include - #include #include +#include "cloisim_ros_bringup/type.h" -using namespace std; +using namespace std::literals::chrono_literals; +using string = std::string; -extern map, tuple>> g_node_map_list; +extern node_map_t g_node_map_list; -void make_bringup_list(const Json::Value& result_map, const string target_model, const string target_parts_type, const string target_parts_name, const bool is_single_mode); +void make_bringup_list( + const Json::Value& result_map, const string target_model, + const string target_parts_type, const string target_parts_name, const bool is_single_mode); void remove_all_bringup_nodes(rclcpp::Executor& executor, const rclcpp::Logger& logger) { @@ -31,9 +34,9 @@ void remove_all_bringup_nodes(rclcpp::Executor& executor, const rclcpp::Logger& const auto& key = it->first; const auto& value = it->second; - const auto node = get<2>(value); + const auto node = std::get<2>(value); executor.remove_node(node); - const auto node_info = get<0>(key) + "/" + get<1>(key) + "/" + get<2>(key); + const auto node_info = std::get<0>(key) + "/" + std::get<1>(key) + "/" + std::get<2>(key); WARN(logger, "> Node(" << node_info << ") removed"); } @@ -50,16 +53,16 @@ void add_all_bringup_nodes(rclcpp::Executor& executor, const rclcpp::Logger& log const auto& key = it->first; auto& value = it->second; - const auto node_info = get<0>(key) + "/" + get<1>(key) + "/" + get<2>(key); - const auto node = get<2>(value); - if (get<0>(value) == false) + const auto node_info = std::get<0>(key) + "/" + std::get<1>(key) + "/" + std::get<2>(key); + const auto node = std::get<2>(value); + if (std::get<0>(value) == false) { WARN(logger, "New Node added(" << node_info << ")"); executor.add_node(node); - get<0>(value) = true; // set to true for marking added_node + std::get<0>(value) = true; // set to true for marking added_node ++it; } - else if (get<1>(value) == true) + else if (std::get<1>(value) == true) { executor.remove_node(node); g_node_map_list.erase(it++); @@ -73,7 +76,10 @@ void add_all_bringup_nodes(rclcpp::Executor& executor, const rclcpp::Logger& log } } -void bringup_process(std::shared_ptr param_node, rclcpp::Executor& executor, const rclcpp::Logger& logger) +void bringup_process( + std::shared_ptr param_node, + rclcpp::Executor& executor, + const rclcpp::Logger& logger) { const auto bringup_list_map = param_node->GetBringUpList(); if (bringup_list_map.empty()) @@ -93,7 +99,8 @@ void bringup_process(std::shared_ptr param_node, rclc const auto targetPartsName = param_node->TargetPartsName(); // cout << targetModel << ":" << targetPartsType << ":" << targetPartsName << endl; - make_bringup_list(bringup_list_map, targetModel, targetPartsType, targetPartsName, is_single_mode); + make_bringup_list(bringup_list_map, + targetModel, targetPartsType, targetPartsName, is_single_mode); add_all_bringup_nodes(executor, logger); } @@ -127,15 +134,17 @@ int main(int argc, char** argv) // INFO(logger, "bringup_process() size=" << g_node_map_list.size()); if (g_node_map_list.size() == 0) { - INFO(logger, "Failed to connect to the CLOiSim. " - << "Wait " << to_string(waitingTime.count()) << "sec and retry to connnect. " - << "Remained retrial=" << retry_count); - this_thread::sleep_for(waitingTime); + INFO(logger, + "Failed to connect to the CLOiSim. " + << "Wait " << std::to_string(waitingTime.count()) + << "sec and retry to connnect. " + << "Remained retrial=" << retry_count); + std::this_thread::sleep_for(waitingTime); } else { retry_count = maxRetryNum; - this_thread::sleep_for(periodicCheckTime); + std::this_thread::sleep_for(periodicCheckTime); } } @@ -143,7 +152,9 @@ int main(int argc, char** argv) kill(getpid(), SIGINT); }); - WARN_ONCE(logger, "Spinning MultiThreadedExecutor NumOfThread=" << executor.get_number_of_threads()); + WARN_ONCE(logger, + "Spinning MultiThreadedExecutor NumOfThread=" + << executor.get_number_of_threads()); executor.spin(); running_thread = false; 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 738a03e0..5d735e6f 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 @@ -18,41 +18,41 @@ #define CLOISIM_ROS_BRINGUP_PARAM__BRINGUP_PARAM_HPP_ #include - #include - #include #include -using namespace std; +using string = std::string; namespace cloisim_ros { - class BringUpParam : public rclcpp::Node { public: - explicit BringUpParam(const string basename = "cloisim_ros"); + explicit BringUpParam(const std::string basename = "cloisim_ros"); virtual ~BringUpParam(); bool IsSingleMode() const { return is_single_mode; } - string TargetModel() const { return target_model; } - string TargetPartsType() const { return target_parts_type; } - string TargetPartsName() const { return target_parts_name; } + std::string TargetModel() const { return target_model; } + std::string TargetPartsType() const { return target_parts_type; } + std::string TargetPartsName() const { return target_parts_name; } void IsSingleMode(const bool value) { is_single_mode = value; } - void TargetModel(const string value) { target_model = value; } - void TargetPartsType(const string value) { target_parts_type = value; } - void TargetPartsName(const string value) { target_parts_name = value; } + void TargetModel(const std::string value) { target_model = value; } + void TargetPartsType(const std::string value) { target_parts_type = value; } + void TargetPartsName(const std::string value) { target_parts_name = value; } Json::Value GetBringUpList(const bool filterByParameters = false); Json::Value RequestBringUpList(); - static bool IsRobotSpecificType(const string node_type); - static bool IsWorldSpecificType(const string node_type); + static bool IsRobotSpecificType(const std::string node_type); + static bool IsWorldSpecificType(const std::string node_type); + + void StoreFilteredInfoAsParameters( + const Json::Value item, rclcpp::NodeOptions &node_options); - void StoreFilteredInfoAsParameters(const Json::Value item, rclcpp::NodeOptions &node_options); - static void StoreBridgeInfosAsParameters(const Json::Value item, rclcpp::NodeOptions &node_options); + static void StoreBridgeInfosAsParameters( + const Json::Value item, rclcpp::NodeOptions &node_options); bool HasEnableTFforMicom() { return has_parameter("micom.enable_tf"); } bool EnableTFforMicom() { return enable_tf_micom_; } @@ -62,9 +62,9 @@ class BringUpParam : public rclcpp::Node private: bool is_single_mode; - string target_model; - string target_parts_type; - string target_parts_name; + std::string target_model; + std::string target_parts_type; + std::string target_parts_name; bool enable_tf_micom_; WebSocketService *ws_service_ptr_; diff --git a/cloisim_ros_bringup_param/src/bringup_param.cpp b/cloisim_ros_bringup_param/src/bringup_param.cpp index 50f28ceb..b6afc015 100644 --- a/cloisim_ros_bringup_param/src/bringup_param.cpp +++ b/cloisim_ros_bringup_param/src/bringup_param.cpp @@ -4,7 +4,7 @@ * @author Hyunseok Yang * @brief * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea * All Rights are Reserved. @@ -14,6 +14,9 @@ #include "cloisim_ros_bringup_param/bringup_param.hpp" +using std::cout; +using std::endl; + namespace cloisim_ros { @@ -57,8 +60,9 @@ BringUpParam::BringUpParam(const string basename) } RCLCPP_INFO_ONCE(this->get_logger(), - "Params > single_mode(%d) target_model(%s) target_parts_type(%s) target_parts_name(%s)", - is_single_mode, target_model.c_str(), target_parts_type.c_str(), target_parts_name.c_str()); + "Params > single_mode(%d) target[model(%s) parts[type(%s) name(%s)]]", + is_single_mode, + target_model.c_str(), target_parts_type.c_str(), target_parts_name.c_str()); RCLCPP_INFO_ONCE(this->get_logger(), "enable_tf > micom(%d)", @@ -202,7 +206,8 @@ Json::Value BringUpParam::GetFilteredListByParameters(const Json::Value result) const auto node_name = node_list.begin().key().asString(); // cout << target_parts_name << ", node_name: " << node_name << endl; - if (target_parts_name.empty() || (!target_parts_name.empty() && target_parts_name.compare(node_name) == 0)) + if (target_parts_name.empty() || + (!target_parts_name.empty() && target_parts_name.compare(node_name) == 0)) root[node_namespace] = node_list; else root[node_namespace] = Json::Value(); @@ -246,7 +251,8 @@ void BringUpParam::StoreFilteredInfoAsParameters( const auto model_name = TargetModel(); const auto isResultEmpty = item[model_name].empty(); - const auto first_iteration = (isResultEmpty) ? Json::ValueConstIterator() : item[model_name].begin(); + const auto first_iteration = + (isResultEmpty) ? Json::ValueConstIterator() : item[model_name].begin(); if (!isResultEmpty) TargetPartsName(first_iteration.key().asString()); @@ -264,4 +270,4 @@ void BringUpParam::StoreFilteredInfoAsParameters( cloisim_ros::BringUpParam::StoreBridgeInfosAsParameters(bridge_infos, node_options); } -} // namespace cloisim_ros \ 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 055da515..67a6e002 100644 --- a/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/camera.hpp @@ -6,7 +6,7 @@ * @brief * ROS2 Camera class for cloisim * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -31,9 +31,13 @@ namespace cloisim_ros class Camera : public CameraBase { 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_ = ""); + 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(); }; } // namespace cloisim_ros -#endif // CLOISIM_ROS_CAMERA__CAMERA_HPP_ \ No newline at end of file + +#endif // CLOISIM_ROS_CAMERA__CAMERA_HPP_ diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp index 8301de6c..3b49f543 100644 --- a/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/camera_base.hpp @@ -5,7 +5,7 @@ * @brief * ROS2 CameraBase class * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -30,7 +30,9 @@ namespace cloisim_ros class CameraBase : public Base { public: - explicit CameraBase(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit CameraBase( + const rclcpp::NodeOptions &options_, + const std::string node_name, const std::string namespace_ = ""); explicit CameraBase(const std::string node_name, const std::string namespace_ = ""); virtual ~CameraBase(); diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/depth_camera.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/depth_camera.hpp index fa6d9037..052ee879 100644 --- a/cloisim_ros_camera/include/cloisim_ros_camera/depth_camera.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/depth_camera.hpp @@ -6,14 +6,14 @@ * @brief * ROS2 Depth Camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ -#ifndef CLOISIM_ROS_CAMERA__DEPTHCAMERA_HPP_ -#define CLOISIM_ROS_CAMERA__DEPTHCAMERA_HPP_ +#ifndef CLOISIM_ROS_CAMERA__DEPTH_CAMERA_HPP_ +#define CLOISIM_ROS_CAMERA__DEPTH_CAMERA_HPP_ #include @@ -25,8 +25,12 @@ namespace cloisim_ros class DepthCamera : public CameraBase { 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_ = ""); + 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: @@ -37,4 +41,5 @@ class DepthCamera : public CameraBase rclcpp::Publisher::SharedPtr pubPointCloud; }; } // namespace cloisim_ros -#endif // CLOISIM_ROS_CAMERA__DEPTHCAMERA_HPP_ + +#endif // CLOISIM_ROS_CAMERA__DEPTH_CAMERA_HPP_ diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/segmentation_camera.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/segmentation_camera.hpp index b8d81a54..53e80572 100644 --- a/cloisim_ros_camera/include/cloisim_ros_camera/segmentation_camera.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/segmentation_camera.hpp @@ -5,7 +5,7 @@ * @brief * ROS2 Segmentation Camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -15,9 +15,9 @@ #ifndef CLOISIM_ROS_CAMERA__SEGMENTATION_CAMERA_HPP_ #define CLOISIM_ROS_CAMERA__SEGMENTATION_CAMERA_HPP_ +#include #include #include -#include #include namespace cloisim_ros @@ -25,8 +25,12 @@ namespace cloisim_ros class SegmentationCamera : public CameraBase { public: - explicit SegmentationCamera(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); - explicit SegmentationCamera(const std::string node_name = "cloisim_ros_segmentationcamera", const std::string namespace_ = ""); + explicit SegmentationCamera( + const rclcpp::NodeOptions &options_, + const std::string node_name, const std::string namespace_ = ""); + explicit SegmentationCamera( + const std::string node_name = "cloisim_ros_segmentationcamera", + const std::string namespace_ = ""); virtual ~SegmentationCamera(); private: @@ -57,4 +61,4 @@ class SegmentationCamera : public CameraBase }; } // namespace cloisim_ros -#endif // CLOISIM_ROS_CAMERA__SEGMENTATION_CAMERA_HPP_ \ No newline at end of file +#endif // CLOISIM_ROS_CAMERA__SEGMENTATION_CAMERA_HPP_ diff --git a/cloisim_ros_camera/include/cloisim_ros_camera/standalone.hpp b/cloisim_ros_camera/include/cloisim_ros_camera/standalone.hpp index 99d1b953..d92d5fcb 100644 --- a/cloisim_ros_camera/include/cloisim_ros_camera/standalone.hpp +++ b/cloisim_ros_camera/include/cloisim_ros_camera/standalone.hpp @@ -5,7 +5,7 @@ * @brief * rclcpp init for standalone * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright (c) 2024 LG Electronics Inc., LTD., Seoul, Korea * All Rights are Reserved. @@ -13,6 +13,11 @@ * SPDX-License-Identifier: MIT */ +#ifndef CLOISIM_ROS_CAMERA__STANDALONE_HPP_ +#define CLOISIM_ROS_CAMERA__STANDALONE_HPP_ + +#include +#include #include template @@ -46,3 +51,5 @@ static void run_standalone_single_executor( executor.add_node(node); executor.spin(); } + +#endif // CLOISIM_ROS_CAMERA__STANDALONE_HPP_ diff --git a/cloisim_ros_camera/src/camera.cpp b/cloisim_ros_camera/src/camera.cpp index 412d6af9..75f319f2 100644 --- a/cloisim_ros_camera/src/camera.cpp +++ b/cloisim_ros_camera/src/camera.cpp @@ -5,22 +5,21 @@ * @brief * ROS2 Camera class * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ #include -#include #include - +#include #include #include #include -using namespace std; -using namespace std::chrono_literals; +using namespace std::literals::chrono_literals; +using string = std::string; namespace cloisim_ros { diff --git a/cloisim_ros_camera/src/camera_base.cpp b/cloisim_ros_camera/src/camera_base.cpp index 182beabb..2954e340 100644 --- a/cloisim_ros_camera/src/camera_base.cpp +++ b/cloisim_ros_camera/src/camera_base.cpp @@ -5,22 +5,22 @@ * @brief * ROS2 CameraBase class * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ #include -#include #include - +#include #include #include #include -using namespace std; -using namespace std::chrono_literals; +using namespace std::placeholders; +using namespace std::literals::chrono_literals; +using std::string; namespace cloisim_ros { @@ -86,7 +86,8 @@ void CameraBase::InitializeCameraInfo() optical_frame_transform_pose.set_name(optical_frame_id_); SetStaticTf2(optical_frame_transform_pose, frame_id_); - camera_info_manager_ = std::make_shared(GetNode().get()); + camera_info_manager_ = + std::make_shared(GetNode().get()); const auto camSensorMsg = GetCameraSensorMessage(info_bridge_ptr); SetCameraInfoInManager(camera_info_manager_, camSensorMsg, frame_id_); } @@ -116,10 +117,11 @@ void CameraBase::InitializeCameraData() if (data_bridge_ptr != nullptr) { data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portData, hashKeyData); - AddPublisherThread(data_bridge_ptr, - bind(static_cast(&CameraBase::PublishData), - this, - std::placeholders::_1)); + AddPublisherThread( + data_bridge_ptr, + bind(static_cast(&CameraBase::PublishData), + this, + std::placeholders::_1)); } DBG_SIM_INFO("hashKey: data(%s)", hashKeyData.c_str()); diff --git a/cloisim_ros_camera/src/depth_camera.cpp b/cloisim_ros_camera/src/depth_camera.cpp index 440f5dd4..8fd25926 100644 --- a/cloisim_ros_camera/src/depth_camera.cpp +++ b/cloisim_ros_camera/src/depth_camera.cpp @@ -6,7 +6,7 @@ * @brief * ROS2 Depth Camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -14,7 +14,7 @@ #include "cloisim_ros_camera/depth_camera.hpp" -using namespace std; +using string = std::string; namespace cloisim_ros { diff --git a/cloisim_ros_camera/src/main_cam.cpp b/cloisim_ros_camera/src/main_cam.cpp index d2561d40..eb14e55d 100644 --- a/cloisim_ros_camera/src/main_cam.cpp +++ b/cloisim_ros_camera/src/main_cam.cpp @@ -5,7 +5,7 @@ * @brief * ROS2 Camera class for cloisim * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea * All Rights are Reserved. @@ -22,4 +22,4 @@ int main(int argc, char** argv) run_standalone_single_executor("cloisim_ros_camera", "CAMERA"); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_camera/src/main_depthcam.cpp b/cloisim_ros_camera/src/main_depthcam.cpp index 9f6b416a..9f6c053f 100644 --- a/cloisim_ros_camera/src/main_depthcam.cpp +++ b/cloisim_ros_camera/src/main_depthcam.cpp @@ -5,7 +5,7 @@ * @brief * ROS2 Camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea * All Rights are Reserved. @@ -19,7 +19,8 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - run_standalone_single_executor("cloisim_ros_depthcamera", "DEPTHCAMERA"); + run_standalone_single_executor( + "cloisim_ros_depthcamera", "DEPTHCAMERA"); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_camera/src/main_segcam.cpp b/cloisim_ros_camera/src/main_segcam.cpp index ed4e0196..e6566692 100644 --- a/cloisim_ros_camera/src/main_segcam.cpp +++ b/cloisim_ros_camera/src/main_segcam.cpp @@ -5,7 +5,7 @@ * @brief * ROS2 Segmentation Camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -18,7 +18,8 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - run_standalone_single_executor("cloisim_ros_segmentationcamera", "SEGMENTCAMERA"); + run_standalone_single_executor( + "cloisim_ros_segmentationcamera", "SEGMENTCAMERA"); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_camera/src/segmentation_camera.cpp b/cloisim_ros_camera/src/segmentation_camera.cpp index c331febc..0130ab80 100644 --- a/cloisim_ros_camera/src/segmentation_camera.cpp +++ b/cloisim_ros_camera/src/segmentation_camera.cpp @@ -5,7 +5,7 @@ * @brief * ROS2 Segmentation Camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -14,7 +14,8 @@ #include "cloisim_ros_camera/segmentation_camera.hpp" -using namespace std; +using namespace std::placeholders; +using string = std::string; namespace cloisim_ros { @@ -51,7 +52,7 @@ void SegmentationCamera::InitializeCameraData() if (data_bridge_ptr != nullptr) { data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portData, hashKeyData); - AddPublisherThread(data_bridge_ptr, bind(&SegmentationCamera::PublishData, this, std::placeholders::_1)); + AddPublisherThread(data_bridge_ptr, bind(&SegmentationCamera::PublishData, this, _1)); } pub_labelinfo_ = create_publisher( 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 c4632850..8a77ccf7 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 @@ -47,15 +47,24 @@ class ElevatorSystem : public Base 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_; + 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( diff --git a/cloisim_ros_elevator_system/package.xml b/cloisim_ros_elevator_system/package.xml index 450616f6..c2b29a7f 100644 --- a/cloisim_ros_elevator_system/package.xml +++ b/cloisim_ros_elevator_system/package.xml @@ -10,7 +10,7 @@ ament_cmake - elevator_system_msgs + elevator_system_msgs cloisim_ros_bringup_param cloisim_ros_base diff --git a/cloisim_ros_elevator_system/src/elevator_system.cpp b/cloisim_ros_elevator_system/src/elevator_system.cpp index 899c7ff1..43aa4a96 100644 --- a/cloisim_ros_elevator_system/src/elevator_system.cpp +++ b/cloisim_ros_elevator_system/src/elevator_system.cpp @@ -15,10 +15,10 @@ #include "cloisim_ros_elevator_system/elevator_system.hpp" -using namespace std; using namespace std::placeholders; -using namespace cloisim; using namespace elevator_system_msgs; +using std::shared_ptr; +using string = std::string; namespace cloisim_ros { @@ -60,7 +60,7 @@ void ElevatorSystem::Initialize() if (reply.IsInitialized()) { if (reply.name().compare("request_system_name") == 0 && - reply.value().type() == msgs::Any_ValueType_STRING && + reply.value().type() == cloisim::msgs::Any_ValueType_STRING && !reply.value().string_value().empty()) { const auto system_name = reply.value().string_value(); @@ -119,9 +119,9 @@ void ElevatorSystem::CallElevator( } void ElevatorSystem::GetElevatorCalled( - const shared_ptr /*request_header*/, - const shared_ptr request, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) { auto message = CreateRequest("get_called_elevator", request->current_floor, @@ -135,23 +135,26 @@ void ElevatorSystem::GetElevatorCalled( const auto result_param = reply.children(2); if (result_param.IsInitialized()) { - const auto elevator_index = (result_param.name().compare("elevator_index") != 0) ? "" : result_param.value().string_value(); + const auto elevator_index = + (result_param.name().compare("elevator_index") != 0) + ? "" + : result_param.value().string_value(); response->elevator_index = elevator_index; } } } void ElevatorSystem::GetElevatorInfo( - const shared_ptr /*request_header*/, - const shared_ptr request, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) { auto message = CreateRequest("get_elevator_information", request->elevator_index); const auto reply = RequestReplyMessage(control_bridge_ptr, message); if (reply.IsInitialized()) { - msgs::Param result_param; + cloisim::msgs::Param result_param; response->result = GetResultFromResponse(reply); @@ -159,7 +162,9 @@ void ElevatorSystem::GetElevatorInfo( if (result_param.IsInitialized()) { const auto current_floor = - (result_param.name().compare("current_floor") != 0) ? "" : result_param.value().string_value(); + (result_param.name().compare("current_floor") != 0) + ? "" + : result_param.value().string_value(); response->current_floor = current_floor; } @@ -167,16 +172,18 @@ void ElevatorSystem::GetElevatorInfo( if (result_param.IsInitialized()) { const auto height = - static_cast((result_param.name().compare("height") != 0) ? 0.0 : result_param.value().double_value()); + static_cast((result_param.name().compare("height") != 0) + ? 0.0 + : result_param.value().double_value()); response->height = height; } } } void ElevatorSystem::SelectFloor( - const shared_ptr /*request_header*/, - const shared_ptr request, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) { auto message = CreateRequest("select_elevator_floor", request->current_floor, @@ -191,9 +198,9 @@ void ElevatorSystem::SelectFloor( } void ElevatorSystem::RequestDoorOpen( - const shared_ptr /*request_header*/, - const shared_ptr request, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) { auto message = CreateRequest("request_door_open", request->elevator_index); @@ -205,9 +212,9 @@ void ElevatorSystem::RequestDoorOpen( } void ElevatorSystem::RequestDoorClose( - const shared_ptr /*request_header*/, - const shared_ptr request, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) { auto message = CreateRequest("request_door_close", request->elevator_index); @@ -219,9 +226,9 @@ void ElevatorSystem::RequestDoorClose( } void ElevatorSystem::IsDoorOpened( - const shared_ptr /*request_header*/, - const shared_ptr request, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr request, + const std::shared_ptr response) { auto message = CreateRequest("is_door_opened", request->elevator_index); @@ -233,17 +240,17 @@ void ElevatorSystem::IsDoorOpened( } void ElevatorSystem::ReserveElevator( - const shared_ptr /*request_header*/, - const shared_ptr /*request*/, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + const std::shared_ptr response) { response->result = srv_mode_; } void ElevatorSystem::ReleaseElevator( - const shared_ptr /*request_header*/, - const shared_ptr /*request*/, - const shared_ptr response) + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + const std::shared_ptr response) { response->result = srv_mode_; } @@ -258,7 +265,7 @@ void ElevatorSystem::ReleaseElevator( * * @return created protobuf message for request */ -msgs::Param ElevatorSystem::CreateRequest( +cloisim::msgs::Param ElevatorSystem::CreateRequest( const string service_name, const string current_floor, const string target_floor, @@ -266,37 +273,39 @@ msgs::Param ElevatorSystem::CreateRequest( { request_msg_.clear_children(); - msgs::Param *param_ptr; - msgs::Any *value_ptr; + cloisim::msgs::Param *param_ptr; + cloisim::msgs::Any *value_ptr; param_ptr = request_msg_.add_children(); param_ptr->set_name("service_name"); value_ptr = param_ptr->mutable_value(); - value_ptr->set_type(msgs::Any::STRING); + value_ptr->set_type(cloisim::msgs::Any::STRING); value_ptr->set_string_value(service_name); param_ptr = request_msg_.add_children(); param_ptr->set_name("current_floor"); value_ptr = param_ptr->mutable_value(); - value_ptr->set_type(msgs::Any::STRING); + value_ptr->set_type(cloisim::msgs::Any::STRING); value_ptr->set_string_value(current_floor); param_ptr = request_msg_.add_children(); param_ptr->set_name("target_floor"); value_ptr = param_ptr->mutable_value(); - value_ptr->set_type(msgs::Any::STRING); + value_ptr->set_type(cloisim::msgs::Any::STRING); value_ptr->set_string_value(target_floor); param_ptr = request_msg_.add_children(); param_ptr->set_name("elevator_index"); value_ptr = param_ptr->mutable_value(); - value_ptr->set_type(msgs::Any::STRING); + value_ptr->set_type(cloisim::msgs::Any::STRING); value_ptr->set_string_value(elevator_index); return request_msg_; } -bool ElevatorSystem::GetResultFromResponse(const msgs::Param &response_msg, const int children_index) +bool ElevatorSystem::GetResultFromResponse( + const cloisim::msgs::Param &response_msg, + const int children_index) { const auto result_param = response_msg.children(children_index); if (!result_param.IsInitialized() || result_param.name().compare("result") != 0) diff --git a/cloisim_ros_elevator_system/src/main.cpp b/cloisim_ros_elevator_system/src/main.cpp index 5afb4e1d..3e31f6fc 100644 --- a/cloisim_ros_elevator_system/src/main.cpp +++ b/cloisim_ros_elevator_system/src/main.cpp @@ -29,7 +29,8 @@ int main(int argc, char *argv[]) rclcpp::executors::SingleThreadedExecutor executor; rclcpp::sleep_for(1000ms); - const auto bringup_param_node = std::make_shared("cloisim_ros_elevator_system"); + const auto bringup_param_node = + std::make_shared("cloisim_ros_elevator_system"); executor.add_node(bringup_param_node); bringup_param_node->IsSingleMode(true); bringup_param_node->TargetPartsType("ELEVATOR"); diff --git a/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp b/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp index 550931ce..dcc63f32 100644 --- a/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp +++ b/cloisim_ros_gps/include/cloisim_ros_gps/gps.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -30,7 +30,9 @@ 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 rclcpp::NodeOptions &options_, + const std::string node_name, const std::string namespace_ = ""); explicit Gps(const std::string namespace_ = ""); ~Gps(); diff --git a/cloisim_ros_gps/src/gps.cpp b/cloisim_ros_gps/src/gps.cpp index 472f1fca..4659eb80 100644 --- a/cloisim_ros_gps/src/gps.cpp +++ b/cloisim_ros_gps/src/gps.cpp @@ -16,7 +16,7 @@ #include "cloisim_ros_gps/gps.hpp" #include -using namespace std; +using string = std::string; namespace cloisim_ros { @@ -79,14 +79,19 @@ void Gps::Initialize() msg_gps_.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX; msg_gps_.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - fill(begin(msg_heading_.orientation_covariance), end(msg_heading_.orientation_covariance), 0.0); - fill(begin(msg_heading_.angular_velocity_covariance), end(msg_heading_.angular_velocity_covariance), 0.0); - fill(begin(msg_heading_.linear_acceleration_covariance), end(msg_heading_.linear_acceleration_covariance), 0.0); + std::fill(begin(msg_heading_.orientation_covariance), + end(msg_heading_.orientation_covariance), 0.0); + std::fill(begin(msg_heading_.angular_velocity_covariance), + end(msg_heading_.angular_velocity_covariance), 0.0); + std::fill(begin(msg_heading_.linear_acceleration_covariance), + end(msg_heading_.linear_acceleration_covariance), 0.0); // ROS2 Publisher - pub_gps_ = this->create_publisher(topic_name_, rclcpp::SensorDataQoS()); + pub_gps_ = + this->create_publisher(topic_name_, rclcpp::SensorDataQoS()); - pub_heading_ = this->create_publisher(topic_name_heading_, rclcpp::SensorDataQoS()); + pub_heading_ = + this->create_publisher(topic_name_heading_, rclcpp::SensorDataQoS()); if (data_bridge_ptr != nullptr) { @@ -117,9 +122,15 @@ void Gps::PublishData(const string &buffer) if (pb_buf_gps_.has_heading()) { - ConvertCLOiSimToRos2(pb_buf_gps_.heading().orientation(), msg_heading_.orientation); - ConvertCLOiSimToRos2(pb_buf_gps_.heading().angular_velocity(), msg_heading_.angular_velocity); - ConvertCLOiSimToRos2(pb_buf_gps_.heading().linear_acceleration(), msg_heading_.linear_acceleration); + ConvertCLOiSimToRos2( + pb_buf_gps_.heading().orientation(), + msg_heading_.orientation); + ConvertCLOiSimToRos2( + pb_buf_gps_.heading().angular_velocity(), + msg_heading_.angular_velocity); + ConvertCLOiSimToRos2( + pb_buf_gps_.heading().linear_acceleration(), + msg_heading_.linear_acceleration); } pub_heading_->publish(msg_heading_); diff --git a/cloisim_ros_ground_truth/README.md b/cloisim_ros_ground_truth/README.md index eb24c107..0392be09 100644 --- a/cloisim_ros_ground_truth/README.md +++ b/cloisim_ros_ground_truth/README.md @@ -5,15 +5,37 @@ ros2 run cloisim_ros_ground_truth standalone ros2 run cloisim_ros_ground_truth standalone --ros-args -p target_parts_name:=tracking ``` -## How to monitor the message of topic. +## How to configure -```shell -ros2 topic echo /ground_truth +if you want to tracking dynamic props, set `enable` attribute in `` element to `true`. + +```xml + + 5 + + box + cylinder + sphere + + + L9S + Former1 + + ``` +### samples + You need to configure the ground truth list from [here](https://github.com/lge-ros2/sample_resources/blob/415a700c5280286a28690cf032cd0f4aa826a150/worlds/lg_seocho_with_actors.world#L301). -## Examples of outputs + +## How to monitor the message of topic + +```shell +ros2 topic echo /ground_truth +``` + +### Examples of outputs ```shell --- 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 f09c0543..b0d19aaa 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 @@ -30,7 +30,7 @@ class GroundTruth : public Base { public: explicit GroundTruth(const rclcpp::NodeOptions &options_, const std::string node_name); - explicit GroundTruth(); + GroundTruth(); virtual ~GroundTruth(); private: diff --git a/cloisim_ros_ground_truth/package.xml b/cloisim_ros_ground_truth/package.xml index 95c2ce83..5d2dd747 100644 --- a/cloisim_ros_ground_truth/package.xml +++ b/cloisim_ros_ground_truth/package.xml @@ -10,7 +10,7 @@ ament_cmake - perception_msgs + perception_msgs cloisim_ros_bringup_param cloisim_ros_base diff --git a/cloisim_ros_ground_truth/src/ground_truth.cpp b/cloisim_ros_ground_truth/src/ground_truth.cpp index a55dfbe6..a6dbd1a1 100644 --- a/cloisim_ros_ground_truth/src/ground_truth.cpp +++ b/cloisim_ros_ground_truth/src/ground_truth.cpp @@ -15,10 +15,9 @@ */ #include "cloisim_ros_ground_truth/ground_truth.hpp" -#include +#include -using namespace std; -using namespace cloisim; +using string = std::string; namespace cloisim_ros { @@ -46,13 +45,16 @@ void GroundTruth::Initialize() const auto hashKey = GetModelName() + GetPartsName() + "Data"; DBG_SIM_INFO("hashKey: %s", hashKey.c_str()); - pub_ = create_publisher("/ground_truth", rclcpp::QoS(rclcpp::KeepLast(10)).transient_local()); + pub_ = create_publisher( + "/ground_truth", + rclcpp::QoS(rclcpp::KeepLast(10)).transient_local()); auto data_bridge_ptr = CreateBridge(); if (data_bridge_ptr != nullptr) { data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portData, hashKey); - AddPublisherThread(data_bridge_ptr, bind(&GroundTruth::PublishData, this, std::placeholders::_1)); + AddPublisherThread(data_bridge_ptr, + bind(&GroundTruth::PublishData, this, std::placeholders::_1)); } } diff --git a/cloisim_ros_ground_truth/src/main.cpp b/cloisim_ros_ground_truth/src/main.cpp index 797b5420..6fd3f6fa 100644 --- a/cloisim_ros_ground_truth/src/main.cpp +++ b/cloisim_ros_ground_truth/src/main.cpp @@ -21,7 +21,8 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - const auto bringup_param_node = std::make_shared("cloisim_ros_ground_truth"); + const auto bringup_param_node = + std::make_shared("cloisim_ros_ground_truth"); bringup_param_node->IsSingleMode(true); bringup_param_node->TargetPartsType("GROUNDTRUTH"); executor.add_node(bringup_param_node); @@ -45,4 +46,4 @@ int main(int argc, char** argv) executor.spin(); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp b/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp index 0dab3344..0abc5a56 100644 --- a/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp +++ b/cloisim_ros_imu/include/cloisim_ros_imu/imu.hpp @@ -28,7 +28,8 @@ namespace cloisim_ros class Imu : public Base { public: - explicit Imu(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit Imu(const rclcpp::NodeOptions &options_, + const std::string node_name, const std::string namespace_ = ""); explicit Imu(const std::string namespace_ = ""); ~Imu(); diff --git a/cloisim_ros_imu/src/imu.cpp b/cloisim_ros_imu/src/imu.cpp index d71e89fc..7fdac246 100644 --- a/cloisim_ros_imu/src/imu.cpp +++ b/cloisim_ros_imu/src/imu.cpp @@ -13,11 +13,12 @@ * SPDX-License-Identifier: MIT */ -#include "cloisim_ros_imu/imu.hpp" -#include #include -using namespace std; +#include "cloisim_ros_imu/imu.hpp" +#include + +using string = std::string; namespace cloisim_ros { @@ -68,9 +69,12 @@ void Imu::Initialize() SetStaticTf2(transform_pose); } - fill(begin(msg_imu_.orientation_covariance), end(msg_imu_.orientation_covariance), 0.0); - fill(begin(msg_imu_.angular_velocity_covariance), end(msg_imu_.angular_velocity_covariance), 0.0); - fill(begin(msg_imu_.linear_acceleration_covariance), end(msg_imu_.linear_acceleration_covariance), 0.0); + std::fill(begin(msg_imu_.orientation_covariance), + end(msg_imu_.orientation_covariance), 0.0); + std::fill(begin(msg_imu_.angular_velocity_covariance), + end(msg_imu_.angular_velocity_covariance), 0.0); + std::fill(begin(msg_imu_.linear_acceleration_covariance), + end(msg_imu_.linear_acceleration_covariance), 0.0); // ROS2 Publisher pub_ = this->create_publisher(topic_name_, rclcpp::SensorDataQoS()); 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 0d800bd4..664b6196 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 @@ -29,7 +29,9 @@ 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 rclcpp::NodeOptions &options_, + const std::string node_name, const std::string namespace_ = ""); explicit JointControl(const std::string namespace_ = ""); virtual ~JointControl(); diff --git a/cloisim_ros_joint_control/src/joint_control.cpp b/cloisim_ros_joint_control/src/joint_control.cpp index 10420685..31c63051 100644 --- a/cloisim_ros_joint_control/src/joint_control.cpp +++ b/cloisim_ros_joint_control/src/joint_control.cpp @@ -13,23 +13,25 @@ * SPDX-License-Identifier: MIT */ -#include "cloisim_ros_joint_control/joint_control.hpp" #include #include - -#include #include #include -using namespace std; -using namespace chrono_literals; +#include "cloisim_ros_joint_control/joint_control.hpp" +#include + +using namespace std::literals::chrono_literals; using namespace std::placeholders; -using namespace cloisim; +using string = std::string; namespace cloisim_ros { -JointControl::JointControl(const rclcpp::NodeOptions &options_, const string node_name, const string namespace_) +JointControl::JointControl( + const rclcpp::NodeOptions &options_, + const string node_name, + const string namespace_) : Base(node_name, namespace_, options_) , pub_joint_state_(nullptr) , sub_joint_job_(nullptr) @@ -60,7 +62,8 @@ void JointControl::Initialize() 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()); + DBG_SIM_INFO("hashKey: pub(%s) sub(%s) tf(%s)", + hashKeyPub.c_str(), hashKeySub.c_str(), hashKeyTf.c_str()); auto info_bridge_ptr = CreateBridge(); auto data_bridge_ptr = CreateBridge(); @@ -117,7 +120,7 @@ void JointControl::Initialize() string JointControl::MakeCommandMessage( control_msgs::msg::JointJog::ConstSharedPtr msg) { - msgs::JointCmd_V pb_joint_cmds; + cloisim::msgs::JointCmd_V pb_joint_cmds; // const auto duration = msg->duration; const auto use_displacement = (msg->joint_names.size() == msg->displacements.size()); @@ -158,7 +161,7 @@ string JointControl::MakeCommandMessage( void JointControl::PublishData(const string &buffer) { - msgs::JointState_V pb_joint_states; + cloisim::msgs::JointState_V pb_joint_states; if (!pb_joint_states.ParseFromString(buffer)) { DBG_SIM_ERR("Parsing error, size(%d)", buffer.length()); @@ -201,7 +204,7 @@ void JointControl::GetRobotDescription(zmq::Bridge *const bridge_ptr) if (reply.IsInitialized() && (reply.name().compare("description") == 0)) { - if (reply.value().type() == msgs::Any_ValueType_STRING) + if (reply.value().type() == cloisim::msgs::Any_ValueType_STRING) { const auto description = reply.value().string_value(); msg_description_.data = description; diff --git a/cloisim_ros_joint_control/src/main.cpp b/cloisim_ros_joint_control/src/main.cpp index cdf287d3..c06b3fd6 100644 --- a/cloisim_ros_joint_control/src/main.cpp +++ b/cloisim_ros_joint_control/src/main.cpp @@ -21,7 +21,8 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - const auto bringup_param_node = std::make_shared("cloisim_ros_joint_control"); + const auto bringup_param_node = + std::make_shared("cloisim_ros_joint_control"); bringup_param_node->TargetPartsType("JOINTCONTROL"); executor.add_node(bringup_param_node); diff --git a/cloisim_ros_lidar/include/cloisim_ros_lidar/lidar.hpp b/cloisim_ros_lidar/include/cloisim_ros_lidar/lidar.hpp index e82311b0..125ddf0f 100644 --- a/cloisim_ros_lidar/include/cloisim_ros_lidar/lidar.hpp +++ b/cloisim_ros_lidar/include/cloisim_ros_lidar/lidar.hpp @@ -32,7 +32,9 @@ class Lidar : public Base using PointCloud2Pub = rclcpp::Publisher::SharedPtr; public: - explicit Lidar(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit Lidar( + const rclcpp::NodeOptions &options_, + const std::string node_name, const std::string namespace_ = ""); explicit Lidar(const std::string namespace_ = ""); ~Lidar(); diff --git a/cloisim_ros_lidar/src/lidar.cpp b/cloisim_ros_lidar/src/lidar.cpp index 4dcfe049..728b113e 100644 --- a/cloisim_ros_lidar/src/lidar.cpp +++ b/cloisim_ros_lidar/src/lidar.cpp @@ -19,8 +19,8 @@ #include "cloisim_ros_lidar/lidar.hpp" #include -using namespace std; -using namespace cloisim; +using namespace std::placeholders; +using string = std::string; namespace cloisim_ros { @@ -80,11 +80,13 @@ void Lidar::Initialize() // ROS2 Publisher if (output_type.compare("LaserScan") == 0) { - pub_laser_ = this->create_publisher(topic_name_, rclcpp::SensorDataQoS()); + pub_laser_ = + this->create_publisher(topic_name_, rclcpp::SensorDataQoS()); } else if (output_type.compare("PointCloud2") == 0) { - pub_pc2_ = this->create_publisher(topic_name_, rclcpp::SensorDataQoS()); + pub_pc2_ = + this->create_publisher(topic_name_, rclcpp::SensorDataQoS()); } else { @@ -94,7 +96,7 @@ void Lidar::Initialize() if (data_bridge_ptr != nullptr) { data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portData, hashKeyData); - AddPublisherThread(data_bridge_ptr, bind(&Lidar::PublishData, this, std::placeholders::_1)); + AddPublisherThread(data_bridge_ptr, bind(&Lidar::PublishData, this, _1)); } } @@ -104,7 +106,7 @@ string Lidar::GetOutputType(zmq::Bridge *const bridge_ptr) if (reply.IsInitialized() && (reply.name().compare("output_type") == 0) && - reply.has_value() && reply.value().type() == msgs::Any_ValueType_STRING && + reply.has_value() && reply.value().type() == cloisim::msgs::Any_ValueType_STRING && !reply.value().string_value().empty()) { const auto output_type = reply.value().string_value(); @@ -189,12 +191,16 @@ void Lidar::UpdatePointCloudData(const double min_intensity) size_t i, j; // Fill pointcloud with laser scan data, converting spherical to Cartesian - for (j = 0, inclination = pb_buf_.scan().vertical_angle_min(); j < vertical_beam_count; ++j, inclination += vertical_angle_step) + for (j = 0, inclination = pb_buf_.scan().vertical_angle_min(); + j < vertical_beam_count; + ++j, inclination += vertical_angle_step) { auto c_inclination = cos(inclination); auto s_inclination = sin(inclination); - for (i = 0, azimuth = pb_buf_.scan().angle_min(); i < beam_count; ++i, azimuth += angle_step, ++range_iter, ++intensity_iter) + for (i = 0, azimuth = pb_buf_.scan().angle_min(); + i < beam_count; + ++i, azimuth += angle_step, ++range_iter, ++intensity_iter) { auto c_azimuth = cos(azimuth); auto s_azimuth = sin(azimuth); diff --git a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp index 2644792b..9eca7cf0 100644 --- a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp +++ b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp @@ -31,7 +31,9 @@ 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 rclcpp::NodeOptions &options_, + const std::string node_name, const std::string namespace_ = ""); explicit Micom(const std::string namespace_ = ""); virtual ~Micom(); @@ -49,7 +51,10 @@ class Micom : public Base 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(); diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp index fd60ee4c..c7db3125 100644 --- a/cloisim_ros_micom/src/micom.cpp +++ b/cloisim_ros_micom/src/micom.cpp @@ -13,17 +13,17 @@ * SPDX-License-Identifier: MIT */ -#include "cloisim_ros_micom/micom.hpp" #include #include -#include #include #include -using namespace std; -using namespace std::chrono_literals; +#include "cloisim_ros_micom/micom.hpp" +#include + +using namespace std::literals::chrono_literals; using namespace std::placeholders; -using namespace cloisim; +using string = std::string; namespace cloisim_ros { @@ -110,7 +110,8 @@ void Micom::Initialize() AddPublisherThread(tf_bridge_ptr, bind(&Base::GenerateTF, this, std::placeholders::_1)); } - auto callback_sub = [this, data_bridge_ptr](const geometry_msgs::msg::Twist::SharedPtr msg) -> void + auto callback_sub = + [this, data_bridge_ptr](const geometry_msgs::msg::Twist::SharedPtr msg) -> void { const auto msgBuf = MakeControlMessage(msg); SetBufferToSimulator(data_bridge_ptr, msgBuf); @@ -135,7 +136,7 @@ void Micom::ResetOdometryCallback( string Micom::MakeControlMessage( const geometry_msgs::msg::Twist::SharedPtr msg) const { - msgs::Twist twistBuf; // m/s and rad/s + cloisim::msgs::Twist twistBuf; // m/s and rad/s auto linear_ptr = twistBuf.mutable_linear(); auto angular_ptr = twistBuf.mutable_angular(); linear_ptr->set_x(msg->linear.x); @@ -168,7 +169,8 @@ void Micom::PublishData(const string &buffer) SetTime(pb_micom_.time()); - // DBG_SIM_WRN("Simulation time %u %u size(%d)", pb_micom_.time().sec(), pb_micom_.time().nsec(), bufferLength); + // DBG_SIM_WRN("Simulation time %u %u size(%d)", + // pb_micom_.time().sec(), pb_micom_.time().nsec(), bufferLength); UpdateOdom(); UpdateImu(); @@ -199,9 +201,11 @@ void Micom::UpdateOdom() // static int cnt = 0; // if (cnt++ % LOGGING_PERIOD == 0) // { - // DBG_SIM_MSG("Wheel odom x[%+3.5f] y[%+3.5f] theta[%+3.5f] vel_lin[%+3.5f] vel_ang[%+3.5f] time(%f)", - // msg_odom_.pose.pose.position.x, msg_odom_.pose.pose.position.y, pb_micom_.odom().pose().z(), - // msg_odom_.twist.twist.linear.x, msg_odom_.twist.twist.angular.z, step_time); + // DBG_SIM_MSG( + // "Wheel odom x[%+3.5f] y[%+3.5f] theta[%+3.5f] vel[lin(%+3.5f) ang(%+3.5f)] time(%f)", + // msg_odom_.pose.pose.position.x, msg_odom_.pose.pose.position.y, + // pb_micom_.odom().pose().z(), + // msg_odom_.twist.twist.linear.x, msg_odom_.twist.twist.angular.z, step_time); // } SetVector3MessageToGeometry(pb_micom_.odom().twist_linear(), msg_odom_.twist.twist.linear); @@ -226,9 +230,12 @@ void Micom::UpdateImu() SetVector3MessageToGeometry(pb_micom_.imu().angular_velocity(), msg_imu_.angular_velocity); SetVector3MessageToGeometry(pb_micom_.imu().linear_acceleration(), msg_imu_.linear_acceleration); - fill(begin(msg_imu_.orientation_covariance), end(msg_imu_.orientation_covariance), 0.0); - fill(begin(msg_imu_.angular_velocity_covariance), end(msg_imu_.angular_velocity_covariance), 0.0); - fill(begin(msg_imu_.linear_acceleration_covariance), end(msg_imu_.linear_acceleration_covariance), 0.0); + std::fill(begin(msg_imu_.orientation_covariance), + end(msg_imu_.orientation_covariance), 0.0); + std::fill(begin(msg_imu_.angular_velocity_covariance), + end(msg_imu_.angular_velocity_covariance), 0.0); + std::fill(begin(msg_imu_.linear_acceleration_covariance), + end(msg_imu_.linear_acceleration_covariance), 0.0); } void Micom::UpdateBattery() diff --git a/cloisim_ros_multicamera/include/cloisim_ros_multicamera/multicamera.hpp b/cloisim_ros_multicamera/include/cloisim_ros_multicamera/multicamera.hpp index 5b272dfd..379a766d 100644 --- a/cloisim_ros_multicamera/include/cloisim_ros_multicamera/multicamera.hpp +++ b/cloisim_ros_multicamera/include/cloisim_ros_multicamera/multicamera.hpp @@ -6,7 +6,7 @@ * @brief * ROS2 Multi Camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -32,7 +32,9 @@ 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 rclcpp::NodeOptions &options_, + const std::string node_name, const std::string namespace_ = ""); explicit MultiCamera(const std::string namespace_ = ""); virtual ~MultiCamera(); diff --git a/cloisim_ros_multicamera/src/main.cpp b/cloisim_ros_multicamera/src/main.cpp index 154b8c48..3c2b9471 100644 --- a/cloisim_ros_multicamera/src/main.cpp +++ b/cloisim_ros_multicamera/src/main.cpp @@ -5,7 +5,7 @@ * @brief * ROS2 multi camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea * All Rights are Reserved. @@ -21,7 +21,8 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - const auto bringup_param_node = std::make_shared("cloisim_ros_multicamera"); + const auto bringup_param_node = + std::make_shared("cloisim_ros_multicamera"); bringup_param_node->TargetPartsType("MULTICAMERA"); executor.add_node(bringup_param_node); diff --git a/cloisim_ros_multicamera/src/multicamera.cpp b/cloisim_ros_multicamera/src/multicamera.cpp index bb26f0a3..262597de 100644 --- a/cloisim_ros_multicamera/src/multicamera.cpp +++ b/cloisim_ros_multicamera/src/multicamera.cpp @@ -6,27 +6,29 @@ * @brief * ROS2 Multi Camera class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. */ -#include -#include #include +#include #include "cloisim_ros_multicamera/multicamera.hpp" +#include #include -using namespace std; -using namespace std::chrono_literals; -using namespace cloisim; +using namespace std::literals::chrono_literals; +using string = std::string; namespace cloisim_ros { -MultiCamera::MultiCamera(const rclcpp::NodeOptions &options_, const string node_name, const std::string namespace_) +MultiCamera::MultiCamera( + const rclcpp::NodeOptions &options_, + const string node_name, + const std::string namespace_) : Base(node_name, namespace_, options_) { Start(); @@ -50,7 +52,8 @@ void MultiCamera::Initialize() const auto hashKeyData = GetTargetHashKey("Data"); const auto hashKeyInfo = GetTargetHashKey("Info"); - DBG_SIM_INFO("hashKey: data(%s), info(%s)", hashKeyData.c_str(), hashKeyInfo.c_str()); + DBG_SIM_INFO("hashKey: data(%s), info(%s)", + hashKeyData.c_str(), hashKeyInfo.c_str()); auto data_bridge_ptr = CreateBridge(); auto info_bridge_ptr = CreateBridge(); @@ -79,13 +82,15 @@ void MultiCamera::Initialize() pubs_.push_back(it.advertiseCamera(topic_base_name_ + "/image_raw", 1)); - camera_info_manager_.push_back(std::make_shared(GetNode().get())); + camera_info_manager_.push_back( + std::make_shared(GetNode().get())); const auto camSensorMsg = GetCameraSensorMessage(info_bridge_ptr, frame_id); SetCameraInfoInManager(camera_info_manager_.back(), camSensorMsg, frame_id); } data_bridge_ptr->Connect(zmq::Bridge::Mode::SUB, portData, hashKeyData); - AddPublisherThread(data_bridge_ptr, bind(&MultiCamera::PublishData, this, std::placeholders::_1)); + AddPublisherThread(data_bridge_ptr, + bind(&MultiCamera::PublishData, this, std::placeholders::_1)); } } diff --git a/cloisim_ros_protobuf_msgs/msgs/micom.proto b/cloisim_ros_protobuf_msgs/msgs/micom.proto index 3232de23..72eee082 100644 --- a/cloisim_ros_protobuf_msgs/msgs/micom.proto +++ b/cloisim_ros_protobuf_msgs/msgs/micom.proto @@ -2,7 +2,7 @@ * @file micom.proto * @date 2020-10-27 * @author hyunseok Yang - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. diff --git a/cloisim_ros_protobuf_msgs/msgs/perception.proto b/cloisim_ros_protobuf_msgs/msgs/perception.proto index f41b5636..19cd9df3 100644 --- a/cloisim_ros_protobuf_msgs/msgs/perception.proto +++ b/cloisim_ros_protobuf_msgs/msgs/perception.proto @@ -2,7 +2,7 @@ * @file perception.proto * @date 2020-10-27 * @author hyunseok Yang - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. diff --git a/cloisim_ros_protobuf_msgs/msgs/perception_v.proto b/cloisim_ros_protobuf_msgs/msgs/perception_v.proto index 68f6c20b..48b07328 100644 --- a/cloisim_ros_protobuf_msgs/msgs/perception_v.proto +++ b/cloisim_ros_protobuf_msgs/msgs/perception_v.proto @@ -2,7 +2,7 @@ * @file perception.proto * @date 2020-10-27 * @author hyunseok Yang - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. diff --git a/cloisim_ros_protobuf_msgs/msgs/segmentation.proto b/cloisim_ros_protobuf_msgs/msgs/segmentation.proto index 564adf4b..6555781d 100644 --- a/cloisim_ros_protobuf_msgs/msgs/segmentation.proto +++ b/cloisim_ros_protobuf_msgs/msgs/segmentation.proto @@ -2,7 +2,7 @@ * @file segmentation.proto * @date 2024-03-04 * @author hyunseok Yang - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. diff --git a/cloisim_ros_protobuf_msgs/msgs/vision_class.proto b/cloisim_ros_protobuf_msgs/msgs/vision_class.proto index 8192baf3..57b2cf85 100644 --- a/cloisim_ros_protobuf_msgs/msgs/vision_class.proto +++ b/cloisim_ros_protobuf_msgs/msgs/vision_class.proto @@ -2,7 +2,7 @@ * @file vision_class.proto * @date 2024-03-04 * @author hyunseok Yang - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2024 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. diff --git a/cloisim_ros_realsense/include/cloisim_ros_realsense/realsense.hpp b/cloisim_ros_realsense/include/cloisim_ros_realsense/realsense.hpp index 0d145bde..05f7da99 100644 --- a/cloisim_ros_realsense/include/cloisim_ros_realsense/realsense.hpp +++ b/cloisim_ros_realsense/include/cloisim_ros_realsense/realsense.hpp @@ -6,7 +6,7 @@ * @brief * ROS2 realsense class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -32,7 +32,8 @@ 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 rclcpp::NodeOptions& options_, + const std::string node_name, const std::string namespace_ = ""); explicit RealSense(const std::string namespace_ = ""); virtual ~RealSense(); @@ -41,7 +42,8 @@ class RealSense : public Base void Deinitialize() override; private: - void InitializeCam(const std::string module_name, zmq::Bridge* const info_ptr, zmq::Bridge* const data_ptr); + 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); @@ -54,7 +56,9 @@ class RealSense : public Base std::map msg_imgs_; // Camera info managers - std::map> camera_info_managers_; + std::map> + camera_info_managers_; // Image publisher std::map pubs_; diff --git a/cloisim_ros_realsense/src/main.cpp b/cloisim_ros_realsense/src/main.cpp index e831b3c2..3ff6b302 100644 --- a/cloisim_ros_realsense/src/main.cpp +++ b/cloisim_ros_realsense/src/main.cpp @@ -6,7 +6,7 @@ * @brief * ROS2 realsense class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright 2020 LG Electronics Inc. , LTD., Seoul, Korea * All Rights are Reserved. @@ -22,7 +22,8 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - const auto bringup_param_node = std::make_shared("cloisim_ros_realsense"); + const auto bringup_param_node = + std::make_shared("cloisim_ros_realsense"); bringup_param_node->TargetPartsType("REALSENSE"); executor.add_node(bringup_param_node); diff --git a/cloisim_ros_realsense/src/realsense.cpp b/cloisim_ros_realsense/src/realsense.cpp index 006144d9..ff2b8d13 100644 --- a/cloisim_ros_realsense/src/realsense.cpp +++ b/cloisim_ros_realsense/src/realsense.cpp @@ -6,7 +6,7 @@ * @brief * ROS2 realsense class for simulator * @remark - * @warning + * @copyright * LGE Advanced Robotics Laboratory * Copyright(C) 2019 LG Electronics Co., LTD., Seoul, Korea * All Rights are Reserved. @@ -15,18 +15,17 @@ #include #include #include - -#include -#include #include #include "cloisim_ros_realsense/realsense.hpp" +#include +#include #include #include -using namespace std; -using namespace chrono_literals; -using namespace cloisim; +using namespace std::placeholders; +using namespace std::literals::chrono_literals; +using string = std::string; namespace cloisim_ros { @@ -75,8 +74,8 @@ void RealSense::Initialize() uint16_t portData; for (const auto& module : activated_modules_) { - const auto module_type = get<0>(module); - const auto module_name = get<1>(module); + const auto module_type = std::get<0>(module); + const auto module_name = std::get<1>(module); get_parameter_or("bridge." + module_name + "Data", portData, uint16_t(0)); get_parameter_or("bridge." + module_name + "Info", portInfo, uint16_t(0)); @@ -103,7 +102,8 @@ void RealSense::Initialize() } else { - DBG_SIM_ERR("Unknown module type: %s name: %s", module_type.c_str(), module_name.c_str()); + DBG_SIM_ERR("Unknown module type: %s name: %s", + module_type.c_str(), module_name.c_str()); } } } @@ -145,7 +145,7 @@ void RealSense::InitializeCam( if (data_ptr != nullptr) { - AddPublisherThread(data_ptr, bind(&RealSense::PublishImgData, this, data_ptr, placeholders::_1)); + AddPublisherThread(data_ptr, bind(&RealSense::PublishImgData, this, data_ptr, _1)); } } @@ -166,11 +166,12 @@ void RealSense::InitializeImu( // ROS2 Publisher const auto topic_base_name = GetPartsName() + "/" + topic_name_; - pub_imu_ = this->create_publisher(topic_base_name, rclcpp::SensorDataQoS()); + pub_imu_ = + this->create_publisher(topic_base_name, rclcpp::SensorDataQoS()); if (data_ptr != nullptr) { - AddPublisherThread(data_ptr, bind(&RealSense::PublishImuData, this, placeholders::_1)); + AddPublisherThread(data_ptr, bind(&RealSense::PublishImuData, this, _1)); } } @@ -209,15 +210,17 @@ 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().type() == cloisim::msgs::Any_ValueType_STRING && !type.value().string_value().empty() && name.has_value() && - name.value().type() == msgs::Any_ValueType_STRING && + name.value().type() == cloisim::msgs::Any_ValueType_STRING && !name.value().string_value().empty()) { - const auto tuple_module = make_tuple(type.value().string_value(), name.value().string_value()); + const auto tuple_module = + std::make_tuple(type.value().string_value(), name.value().string_value()); activated_modules_.push_back(tuple_module); - moduleListStr.append(get<1>(tuple_module) + "(" + get<0>(tuple_module) + "), "); + moduleListStr.append(std::get<1>(tuple_module) + + "(" + std::get<0>(tuple_module) + "), "); } } } @@ -277,9 +280,12 @@ void RealSense::PublishImuData(const string& buffer) SetVector3MessageToGeometry(pb_buf_.angular_velocity(), msg_imu_.angular_velocity); SetVector3MessageToGeometry(pb_buf_.linear_acceleration(), msg_imu_.linear_acceleration); - fill(begin(msg_imu_.orientation_covariance), end(msg_imu_.orientation_covariance), 0.0); - fill(begin(msg_imu_.angular_velocity_covariance), end(msg_imu_.angular_velocity_covariance), 0.0); - fill(begin(msg_imu_.linear_acceleration_covariance), end(msg_imu_.linear_acceleration_covariance), 0.0); + std::fill(begin(msg_imu_.orientation_covariance), + end(msg_imu_.orientation_covariance), 0.0); + std::fill(begin(msg_imu_.angular_velocity_covariance), + end(msg_imu_.angular_velocity_covariance), 0.0); + std::fill(begin(msg_imu_.linear_acceleration_covariance), + end(msg_imu_.linear_acceleration_covariance), 0.0); pub_imu_->publish(msg_imu_); } diff --git a/cloisim_ros_sonar/include/cloisim_ros_sonar/sonar.hpp b/cloisim_ros_sonar/include/cloisim_ros_sonar/sonar.hpp index e64dd559..9fa75fef 100644 --- a/cloisim_ros_sonar/include/cloisim_ros_sonar/sonar.hpp +++ b/cloisim_ros_sonar/include/cloisim_ros_sonar/sonar.hpp @@ -28,7 +28,8 @@ namespace cloisim_ros class Sonar : public Base { public: - explicit Sonar(const rclcpp::NodeOptions &options_, const std::string node_name, const std::string namespace_ = ""); + explicit Sonar(const rclcpp::NodeOptions &options_, const std::string node_name, + const std::string namespace_ = ""); explicit Sonar(const std::string namespace_ = ""); ~Sonar(); diff --git a/cloisim_ros_sonar/src/sonar.cpp b/cloisim_ros_sonar/src/sonar.cpp index 08a4ad71..b7295cbf 100644 --- a/cloisim_ros_sonar/src/sonar.cpp +++ b/cloisim_ros_sonar/src/sonar.cpp @@ -13,11 +13,12 @@ * SPDX-License-Identifier: MIT */ -#include "cloisim_ros_sonar/sonar.hpp" -#include #include -using namespace std; +#include "cloisim_ros_sonar/sonar.hpp" +#include + +using string = std::string; namespace cloisim_ros { diff --git a/cloisim_ros_websocket_service/src/websocket_service.cpp b/cloisim_ros_websocket_service/src/websocket_service.cpp index c950074c..66068273 100644 --- a/cloisim_ros_websocket_service/src/websocket_service.cpp +++ b/cloisim_ros_websocket_service/src/websocket_service.cpp @@ -14,167 +14,176 @@ #include "cloisim_ros_websocket_service/websocket_service.hpp" -using namespace std; +using namespace std::placeholders; +using namespace std::literals::chrono_literals; +using string = std::string; +using std::cout; +using std::endl; namespace cloisim_ros { namespace { -constexpr auto DEFAULT_WS_SERVICE_PORT = "8080"; -constexpr auto DEFAULT_WS_SERVICE_BRIDGE_ADDRESS = "127.0.0.1"; + constexpr auto DEFAULT_WS_SERVICE_PORT = "8080"; + constexpr auto DEFAULT_WS_SERVICE_BRIDGE_ADDRESS = "127.0.0.1"; } // namespace -WebSocketService::WebSocketService() -{ - const auto env_service_port = getenv("CLOISIM_SERVICE_PORT"); - const auto service_port = string((env_service_port == nullptr) ? DEFAULT_WS_SERVICE_PORT : env_service_port); + WebSocketService::WebSocketService() + { + const auto env_service_port = getenv("CLOISIM_SERVICE_PORT"); + const auto service_port = + string((env_service_port == nullptr) ? DEFAULT_WS_SERVICE_PORT : env_service_port); - new (this) WebSocketService(service_port); -} + new (this) WebSocketService(service_port); + } -WebSocketService::WebSocketService(const string service_port) -{ - const auto env_bridge_ip = getenv("CLOISIM_BRIDGE_IP"); - const auto bridge_ip = string((env_bridge_ip == nullptr) ? DEFAULT_WS_SERVICE_BRIDGE_ADDRESS : env_bridge_ip); - - new (this) WebSocketService(bridge_ip, service_port); -} - -WebSocketService::WebSocketService(const string bridge_ip, const string service_port) - : base_uri_("ws://" + bridge_ip + ":" + service_port) - , thread_(nullptr) - , target_filter("") - , payload_("") - , is_reply_received_(false) - , is_connected_(false) -{ - // Set logging to be pretty verbose (everything except message payloads) - client_.clear_access_channels(websocketpp::log::alevel::all); - client_.clear_error_channels(websocketpp::log::elevel::all); - client_.set_error_channels(websocketpp::log::elevel::rerror); - - // client_.set_access_channels(websocketpp::log::alevel::connect); - // client_.set_access_channels(websocketpp::log::alevel::app); - // client_.set_access_channels(websocketpp::log::alevel::http); - // client_.set_access_channels(websocketpp::log::alevel::access_core); - // client_.set_access_channels(websocketpp::log::alevel::disconnect); - // client_.set_access_channels(websocketpp::log::alevel::fail); - // client_.set_error_channels(websocketpp::log::elevel::all); - // client_.set_error_channels(websocketpp::log::elevel::info); - // client_.set_error_channels(websocketpp::log::elevel::fatal); - - // Register our message handler - client_.set_open_handler(bind(&WebSocketService::on_open, this, placeholders::_1)); - client_.set_close_handler(bind(&WebSocketService::on_close, this, placeholders::_1)); - client_.set_message_handler(bind(&WebSocketService::on_message, this, placeholders::_1, placeholders::_2)); - - client_.set_reuse_addr(true); - - // Initialize ASIO - client_.init_asio(); - client_.start_perpetual(); - - thread_.reset(new websocketpp::lib::thread(&client::run, &client_)); -} - -WebSocketService::~WebSocketService() -{ - // cout << __FUNCTION__ << endl; - client_.stop_perpetual(); - client_.stop(); + WebSocketService::WebSocketService(const string service_port) + { + const auto env_bridge_ip = getenv("CLOISIM_BRIDGE_IP"); + const auto bridge_ip = + string((env_bridge_ip == nullptr) ? DEFAULT_WS_SERVICE_BRIDGE_ADDRESS : env_bridge_ip); - if (thread_->joinable()) - thread_->join(); - thread_ = nullptr; -} + new (this) WebSocketService(bridge_ip, service_port); + } -void WebSocketService::on_message(websocketpp::connection_hdl hdl, client::message_ptr msg) -{ - (void)hdl; - // cout << __FUNCTION__ << ":: DUMP=" << msg->get_payload() << endl; - payload_ = msg->get_payload(); - is_reply_received_ = true; -} + WebSocketService::WebSocketService(const string bridge_ip, const string service_port) + : base_uri_("ws://" + bridge_ip + ":" + service_port) + , thread_(nullptr) + , target_filter("") + , payload_("") + , is_reply_received_(false) + , is_connected_(false) + { + // Set logging to be pretty verbose (everything except message payloads) + client_.clear_access_channels(websocketpp::log::alevel::all); + client_.clear_error_channels(websocketpp::log::elevel::all); + client_.set_error_channels(websocketpp::log::elevel::rerror); + + // client_.set_access_channels(websocketpp::log::alevel::connect); + // client_.set_access_channels(websocketpp::log::alevel::app); + // client_.set_access_channels(websocketpp::log::alevel::http); + // client_.set_access_channels(websocketpp::log::alevel::access_core); + // client_.set_access_channels(websocketpp::log::alevel::disconnect); + // client_.set_access_channels(websocketpp::log::alevel::fail); + // client_.set_error_channels(websocketpp::log::elevel::all); + // client_.set_error_channels(websocketpp::log::elevel::info); + // client_.set_error_channels(websocketpp::log::elevel::fatal); + + // Register our message handler + client_.set_open_handler(bind(&WebSocketService::on_open, this, _1)); + client_.set_close_handler(bind(&WebSocketService::on_close, this, _1)); + client_.set_message_handler(bind(&WebSocketService::on_message, this, _1, _2)); + + client_.set_reuse_addr(true); + + // Initialize ASIO + client_.init_asio(); + client_.start_perpetual(); + + thread_.reset(new websocketpp::lib::thread(&client::run, &client_)); + } -void WebSocketService::on_open(websocketpp::connection_hdl hdl) -{ - (void)hdl; - // cout << __FUNCTION__ << endl; - conn_hdl_ = hdl; - is_connected_ = true; -} + WebSocketService::~WebSocketService() + { + // cout << __FUNCTION__ << endl; + client_.stop_perpetual(); + client_.stop(); -void WebSocketService::on_close(websocketpp::connection_hdl hdl) -{ - (void)hdl; - cout << "Disconnected from CLOiSim" << endl; - is_connected_ = false; -} + if (thread_->joinable()) + thread_->join(); + thread_ = nullptr; + } -void WebSocketService::Request() -{ - if (IsConnected() && is_reply_received_ == false) + void WebSocketService::on_message(websocketpp::connection_hdl hdl, client::message_ptr msg) { - try - { - const auto request_msg = "{'command':'device_list', 'filter':'" + target_filter + "'}"; - client_.send(conn_hdl_, request_msg, websocketpp::frame::opcode::value::TEXT); - // cout << "Request" << endl; - this_thread::sleep_for(300ms); - } - catch (const websocketpp::exception &e) + (void)hdl; + // cout << __FUNCTION__ << ":: DUMP=" << msg->get_payload() << endl; + payload_ = msg->get_payload(); + is_reply_received_ = true; + } + + void WebSocketService::on_open(websocketpp::connection_hdl hdl) + { + (void)hdl; + // cout << __FUNCTION__ << endl; + conn_hdl_ = hdl; + is_connected_ = true; + } + + void WebSocketService::on_close(websocketpp::connection_hdl hdl) + { + (void)hdl; + cout << "Disconnected from CLOiSim" << endl; + is_connected_ = false; + } + + void WebSocketService::Request() + { + if (IsConnected() && is_reply_received_ == false) { - cout << IsConnected() << ", " << is_reply_received_ << endl; - cout << "Exception in Request() because: " << e.what() << endl; - this_thread::sleep_for(1000ms); + try + { + const auto request_msg = "{'command':'device_list', 'filter':'" + target_filter + "'}"; + client_.send(conn_hdl_, request_msg, websocketpp::frame::opcode::value::TEXT); + // cout << "Request" << endl; + std::this_thread::sleep_for(300ms); + } + catch (const websocketpp::exception &e) + { + cout << IsConnected() << ", " << is_reply_received_ << endl; + cout << "Exception in Request() because: " << e.what() << endl; + std::this_thread::sleep_for(1000ms); + } } } -} -std::string WebSocketService::PopPayload() -{ - // cout << "(" << payload_.size() << ") " << payload_ << endl; - const auto payload_to_return = payload_; - payload_.clear(); - is_reply_received_ = false; - return payload_to_return; -} - -void WebSocketService::Run() -{ - if (IsConnected() == false) + std::string WebSocketService::PopPayload() { - const auto target_uri = base_uri_ + "/control"; - // cout << __FUNCTION__ << ", " << target_uri << endl; + // cout << "(" << payload_.size() << ") " << payload_ << endl; + const auto payload_to_return = payload_; + payload_.clear(); + is_reply_received_ = false; + return payload_to_return; + } - try + void WebSocketService::Run() + { + if (IsConnected() == false) { - websocketpp::lib::error_code ec; - const auto conn = client_.get_connection(target_uri, ec); + const auto target_uri = base_uri_ + "/control"; + // cout << __FUNCTION__ << ", " << target_uri << endl; - if (!ec) + try { - // Note that connect here only requests a connection. No network messages are - // exchanged until the event loop starts running in the next line. - client_.connect(conn); + websocketpp::lib::error_code ec; + const auto conn = client_.get_connection(target_uri, ec); + + if (!ec) + { + // Note that connect here only requests a connection. No network messages are + // exchanged until the event loop starts running in the next line. + client_.connect(conn); + } + else + { + cout << "could not create connection because: " << ec.message() + << " target_uri: " << target_uri << endl; + } + } + catch (websocketpp::exception const &e) + { + cout << "Exception:" << e.what() << endl; + } + catch (websocketpp::lib::error_code e) + { + cout << "Error Code: " << e.message() << endl; + } + catch (...) + { + cout << "other exception" << endl; } - else - cout << "could not create connection because: " << ec.message() << " target_uri: " << target_uri << endl; - } - catch (websocketpp::exception const &e) - { - cout << "Exception:" << e.what() << endl; - } - catch (websocketpp::lib::error_code e) - { - cout << "Error Code: " << e.message() << endl; - } - catch (...) - { - cout << "other exception" << endl; - } - this_thread::sleep_for(500ms); + std::this_thread::sleep_for(500ms); + } } -} } // namespace cloisim_ros diff --git a/cloisim_ros_world/src/main.cpp b/cloisim_ros_world/src/main.cpp index 23874cdb..e0cab012 100644 --- a/cloisim_ros_world/src/main.cpp +++ b/cloisim_ros_world/src/main.cpp @@ -40,4 +40,4 @@ int main(int argc, char** argv) executor.spin(); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} diff --git a/cloisim_ros_world/src/world.cpp b/cloisim_ros_world/src/world.cpp index f675036c..b9e5f0e8 100644 --- a/cloisim_ros_world/src/world.cpp +++ b/cloisim_ros_world/src/world.cpp @@ -19,12 +19,11 @@ #include #include -using namespace std; -using namespace cloisim; +using string = std::string; namespace cloisim_ros { -World::World(const rclcpp::NodeOptions &options_, const std::string node_name) +World::World(const rclcpp::NodeOptions &options_, const string node_name) : Base(node_name, options_) { Start(false);