diff --git a/Dockerfile b/Dockerfile index 4189edf4..bc5583b8 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,37 +1,19 @@ -FROM ubuntu:20.04 +FROM ros:foxy-ros-base -ENV HOSTNAME sim-device -ENV ROS_DISTRO=foxy - -RUN apt update && apt upgrade -q -y && \ - apt install -q -y locales curl gnupg2 lsb-release && \ - apt autoclean && rm -rf /var/lib/apt/lists/* - -RUN locale-gen en_US en_US.UTF-8 && \ - update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 && \ - export LANG=en_US.UTF-8 - -RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - - -RUN sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' - -RUN export DEBIAN_FRONTEND=noninteractive && \ - apt update && \ - apt install -y ros-${ROS_DISTRO}-ros-base git python3-colcon-common-extensions g++ && \ - apt install -y python3-websocket libzmq3-dev libprotobuf-dev protobuf-compiler ros-${ROS_DISTRO}-image-transport-plugins ros-${ROS_DISTRO}-camera-info-manager && \ - apt autoclean && rm -rf /var/lib/apt/lists/* - -RUN mkdir -p /opt/lge-ros2/src +ENV HOSTNAME sim_device WORKDIR /opt/lge-ros2/src -RUN git clone https://github.com/lge-ros2/sim-device.git -b ${ROS_DISTRO} +RUN git clone https://github.com/lge-ros2/sim_device.git -b ${ROS_DISTRO} RUN git clone https://github.com/lge-ros2/cloi_common_interfaces.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 ["/bin/bash", "-c", "source /opt/ros/${ROS_DISTRO}/setup.bash; colcon build --symlink-install"] COPY ./.entrypoint.sh /opt/ -ENTRYPOINT ["/opt/.entrypoint.sh"] +ENTRYPOINT ["/opt/.entrypoint.sh"] \ No newline at end of file diff --git a/bringup/launch/unity_ros.launch.py b/bringup/launch/unity_ros.launch.py index 958e4ff3..5111b0c2 100755 --- a/bringup/launch/unity_ros.launch.py +++ b/bringup/launch/unity_ros.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): _package_name = 'unity_ros' - start_unity_ros_init_cmd = Node( + start_unity_ros_cmd = Node( package=_package_name, executable=_package_name, name=_name, @@ -49,6 +49,6 @@ def generate_launch_description(): ld.add_action(declare_launch_argument_nn) # Add the actions to launch all of the navigation nodes - ld.add_action(start_unity_ros_init_cmd) + ld.add_action(start_unity_ros_cmd) return ld diff --git a/bringup/package.xml b/bringup/package.xml index 3fa8b925..6ec2ca05 100644 --- a/bringup/package.xml +++ b/bringup/package.xml @@ -33,6 +33,8 @@ launch launch_testing + python3-websocket + ament_cmake diff --git a/camera_driver_sim/include/camera_driver_sim/CameraDriverSim.hpp b/camera_driver_sim/include/camera_driver_sim/CameraDriverSim.hpp index d4a5e376..0bbb92ba 100644 --- a/camera_driver_sim/include/camera_driver_sim/CameraDriverSim.hpp +++ b/camera_driver_sim/include/camera_driver_sim/CameraDriverSim.hpp @@ -45,10 +45,7 @@ class CameraDriverSim : public DriverSim sensor_msgs::msg::Image msg_img; // Image publisher - image_transport::Publisher pubImage; - - // Camera info publisher - rclcpp::Publisher::SharedPtr pubCameraInfo{nullptr}; + image_transport::CameraPublisher pubImage; // Camera info manager std::shared_ptr cameraInfoManager; diff --git a/camera_driver_sim/src/CameraDriverSim.cpp b/camera_driver_sim/src/CameraDriverSim.cpp index 32bd4448..de981915 100644 --- a/camera_driver_sim/src/CameraDriverSim.cpp +++ b/camera_driver_sim/src/CameraDriverSim.cpp @@ -54,7 +54,6 @@ void CameraDriverSim::Initialize() pSimBridgeData->Connect(SimBridge::Mode::SUB, portData_, m_hashKeySub + "Data"); } - if (pSimBridgeInfo != nullptr) { pSimBridgeInfo->Connect(SimBridge::Mode::CLIENT, portInfo, m_hashKeySub + "Info"); @@ -74,8 +73,7 @@ void CameraDriverSim::Initialize() const auto topic_base_name_ = GetPartsName() + "/" + topic_name_; image_transport::ImageTransport it(GetNode()); - pubImage = it.advertise(topic_base_name_ + "/image_raw", 1); - pubCameraInfo = create_publisher(topic_base_name_ + "/camera_info", 1); + pubImage = it.advertiseCamera(topic_base_name_ + "/image_raw", 1); } void CameraDriverSim::Deinitialize() @@ -92,7 +90,6 @@ void CameraDriverSim::UpdateData(const uint bridge_index) const bool succeeded = GetBufferFromSimulator(bridge_index, &pBuffer, bufferLength); if (!succeeded || bufferLength < 0) { - DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); return; } @@ -115,11 +112,9 @@ void CameraDriverSim::UpdateData(const uint bridge_index) sensor_msgs::fillImage(msg_img, encoding_arg, rows_arg, cols_arg, step_arg, reinterpret_cast(m_pbImgBuf.image().data().data())); - pubImage.publish(msg_img); - // Publish camera info auto camera_info_msg = cameraInfoManager->getCameraInfo(); - camera_info_msg.header.stamp = m_simTime; + camera_info_msg.header.stamp = msg_img.header.stamp; - pubCameraInfo->publish(camera_info_msg); + pubImage.publish(msg_img, camera_info_msg); } \ No newline at end of file diff --git a/depth_camera_driver_sim/CMakeLists.txt b/depth_camera_driver_sim/CMakeLists.txt index 8f10ff0f..c32b9fc2 100644 --- a/depth_camera_driver_sim/CMakeLists.txt +++ b/depth_camera_driver_sim/CMakeLists.txt @@ -19,7 +19,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) find_package(camera_info_manager REQUIRED) -find_package(image_transport REQUIRED) +# find_package(image_transport REQUIRED) find_package(camera_driver_sim REQUIRED) set(dependencies diff --git a/driver_sim/include/driver_sim/helper.h b/driver_sim/include/driver_sim/helper.h index 9fd9ccd5..56490834 100644 --- a/driver_sim/include/driver_sim/helper.h +++ b/driver_sim/include/driver_sim/helper.h @@ -84,16 +84,16 @@ static void SetCameraInfoInManager( sensor_msgs::msg::CameraInfo camera_info_msg; - auto width = msg.image_size().x(); - auto height = msg.image_size().y(); + const auto width = msg.image_size().x(); + const auto height = msg.image_size().y(); // C parameters - auto cx_ = (width + 1.0) / 2.0; - auto cy_ = (height + 1.0) / 2.0; + const auto cx_ = (width + 1.0) / 2.0; + const auto cy_ = (height + 1.0) / 2.0; - double hfov_ = msg.horizontal_fov(); + const auto hfov_ = msg.horizontal_fov(); - auto computed_focal_length = width / (2.0 * tan(hfov_ / 2.0)); + const auto computed_focal_length = width / (2.0 * tan(hfov_ / 2.0)); // CameraInfo camera_info_msg.header.frame_id = frame_id; @@ -116,9 +116,10 @@ static void SetCameraInfoInManager( // Original camera matrix camera_info_msg.k.fill(0.0); camera_info_msg.k[0] = computed_focal_length; - camera_info_msg.k[2] = cx_; camera_info_msg.k[4] = computed_focal_length; + camera_info_msg.k[2] = cx_; camera_info_msg.k[5] = cy_; + camera_info_msg.k[8] = 1.0; // rectification camera_info_msg.r.fill(0.0); diff --git a/driver_sim/package.xml b/driver_sim/package.xml index 9b81a505..072073d3 100644 --- a/driver_sim/package.xml +++ b/driver_sim/package.xml @@ -21,6 +21,8 @@ driver_sim_interfaces sim_bridge + protobuf-dev + ament_cmake diff --git a/driver_sim/src/driver_sim.cpp b/driver_sim/src/driver_sim.cpp index db9e369a..dc317637 100644 --- a/driver_sim/src/driver_sim.cpp +++ b/driver_sim/src/driver_sim.cpp @@ -112,7 +112,7 @@ void DriverSim::PublishTF() void DriverSim::PublishStaticTF() { // Update timestamp - for (auto _tf : m_static_tf_list) + for (auto &_tf : m_static_tf_list) { _tf.header.stamp = m_simTime; } @@ -256,10 +256,9 @@ bool DriverSim::GetBufferFromSimulator(const uint bridge_index, void** ppBbuffer return false; } - const auto succeeded = simBridge->Receive(ppBbuffer, bufferLength, false); + const auto succeeded = simBridge->Receive(ppBbuffer, bufferLength, isNonBlockingMode); if (!succeeded || bufferLength < 0) { - DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); return false; } diff --git a/driver_sim_interfaces/README.md b/driver_sim_interfaces/README.md index dc2f3753..834f3b95 100644 --- a/driver_sim_interfaces/README.md +++ b/driver_sim_interfaces/README.md @@ -1,49 +1,3 @@ # driver sim interfaces -This is 'driver sim' base class. - -You may need to redefine in a derived class three virtual class. - -```c++ -class DriverSim : public rclcpp::Node -{ -public: - explicit DriverSim(const std::string node_name, const int number_of_simbridge = 1); - -protected: - virtual void Initialize() = 0; - virtual void Deinitialize() = 0; - virtual void UpdateData() = 0; - - void Start(); - void Stop(); - - void AddTf2(const geometry_msgs::msg::TransformStamped _tf); - void AddStaticTf2(const geometry_msgs::msg::TransformStamped _tf); - - bool IsRunThread(); - rclcpp::Node* GetNode(); - SimBridge* GetSimBridge(const int bridge_index = 0); - std::string GetRobotName(); - void PublishTF(); -} -``` - -'publisher' and 'subscriber' can be setup simultaneously at one SimBridge module. - -Following bridge combination should be setup through separated SimBridge module. - -- 'publisher' + ('service' or 'client') -- 'subscriber' + ('service' or 'client') -- 'service' + 'client' - -Examples of using separated SimBridge modules when base class DriverSim() is inherited. - -- inherit base class 'DriverSim(node name, number of sim bridge);' -- Call sim bridge using bridge index - - GetSimBridge(0)-> ... - - GetSimBridge(1)-> ... - - GetSimBridge(0)->Connect(SimBridge::Mode::SUB, m_hashKeySub); - - GetSimBridge(1)->Connect(SimBridge::Mode::CLIENT, m_hashKeySub + "Info"); - - GetSimBridge(0)->Disconnect(); - - GetSimBridge(1)->Disconnect(); +define protobuf message format \ No newline at end of file diff --git a/elevator_system_sim/CMakeLists.txt b/elevator_system_sim/CMakeLists.txt index ef25da16..881c8855 100644 --- a/elevator_system_sim/CMakeLists.txt +++ b/elevator_system_sim/CMakeLists.txt @@ -18,13 +18,12 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(driver_sim_interfaces REQUIRED) find_package(sim_bridge REQUIRED) find_package(elevator_system_msgs REQUIRED) +find_package(driver_sim_interfaces REQUIRED) set(dependencies rclcpp_lifecycle - rosgraph_msgs elevator_system_msgs sim_bridge driver_sim_interfaces diff --git a/elevator_system_sim/package.xml b/elevator_system_sim/package.xml index fa1d0d5c..a2c3d926 100644 --- a/elevator_system_sim/package.xml +++ b/elevator_system_sim/package.xml @@ -12,13 +12,13 @@ ament_cmake - rclcpp - rclcpp_lifecycle - sensor_msgs - geometry_msgs - elevator_system_msgs - driver_sim - camera_driver_sim + rclcpp + rclcpp_lifecycle + driver_sim_interfaces + elevator_system_msgs + sim_bridge + + driver_sim_interfaces ament_lint_auto ament_lint_common diff --git a/elevator_system_sim/src/ElevatorSystemSim.cpp b/elevator_system_sim/src/ElevatorSystemSim.cpp index 29f1d44a..2b187dcd 100644 --- a/elevator_system_sim/src/ElevatorSystemSim.cpp +++ b/elevator_system_sim/src/ElevatorSystemSim.cpp @@ -22,6 +22,7 @@ ElevatorSystemSim::ElevatorSystemSim(bool intra_process_comms) : LifecycleNode("elevator_system_sim", rclcpp::NodeOptions() .allow_undeclared_parameters(true) + .parameter_overrides(vector{rclcpp::Parameter("use_sim_time", true)}) .automatically_declare_parameters_from_overrides(true) .use_intra_process_comms(intra_process_comms)) , m_pSimBridge(new SimBridge()) diff --git a/gps_driver_sim/src/GPSDriverSim.cpp b/gps_driver_sim/src/GPSDriverSim.cpp index b83b3f37..56551748 100644 --- a/gps_driver_sim/src/GPSDriverSim.cpp +++ b/gps_driver_sim/src/GPSDriverSim.cpp @@ -90,7 +90,6 @@ void GPSDriverSim::UpdateData(const uint bridge_index) const bool succeeded = GetBufferFromSimulator(bridge_index, &pBuffer, bufferLength); if (!succeeded || bufferLength < 0) { - DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); return; } diff --git a/lidar_driver_sim/package.xml b/lidar_driver_sim/package.xml index ae603079..ca3248f0 100644 --- a/lidar_driver_sim/package.xml +++ b/lidar_driver_sim/package.xml @@ -12,7 +12,6 @@ rclcpp sensor_msgs geometry_msgs - image_transport driver_sim diff --git a/lidar_driver_sim/src/LidarDriverSim.cpp b/lidar_driver_sim/src/LidarDriverSim.cpp index 3bf83cba..f4618ae9 100644 --- a/lidar_driver_sim/src/LidarDriverSim.cpp +++ b/lidar_driver_sim/src/LidarDriverSim.cpp @@ -79,8 +79,7 @@ void LidarDriverSim::UpdateData(const uint bridge_index) const bool succeeded = GetBufferFromSimulator(bridge_index, &pBuffer, bufferLength); if (!succeeded || bufferLength < 0) - { - DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); + { return; } diff --git a/micom_driver_sim/src/MicomDriverSim.cpp b/micom_driver_sim/src/MicomDriverSim.cpp index 2a0b143d..386129c7 100644 --- a/micom_driver_sim/src/MicomDriverSim.cpp +++ b/micom_driver_sim/src/MicomDriverSim.cpp @@ -321,7 +321,6 @@ void MicomDriverSim::UpdateData(const uint bridge_index) const bool succeeded = GetBufferFromSimulator(bridge_index, &pBuffer, bufferLength); if (!succeeded || bufferLength < 0) { - DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); return; } diff --git a/multi_camera_driver_sim/include/multi_camera_driver_sim/MultiCameraDriverSim.hpp b/multi_camera_driver_sim/include/multi_camera_driver_sim/MultiCameraDriverSim.hpp index cdcee182..86e8299d 100644 --- a/multi_camera_driver_sim/include/multi_camera_driver_sim/MultiCameraDriverSim.hpp +++ b/multi_camera_driver_sim/include/multi_camera_driver_sim/MultiCameraDriverSim.hpp @@ -46,15 +46,12 @@ class MultiCameraDriverSim : public DriverSim gazebo::msgs::ImagesStamped m_pbBuf; // message for ROS2 communictaion - sensor_msgs::msg::Image msg_img; - - // Camera info publishers. - std::vector::SharedPtr> pubCamerasInfo; + std::map msg_imgs_; // Camera info managers std::vector> cameraInfoManager; // Image publisher - std::vector pubImages; + std::vector pubImages_; }; #endif diff --git a/multi_camera_driver_sim/src/MultiCameraDriverSim.cpp b/multi_camera_driver_sim/src/MultiCameraDriverSim.cpp index cda5b85c..29465286 100644 --- a/multi_camera_driver_sim/src/MultiCameraDriverSim.cpp +++ b/multi_camera_driver_sim/src/MultiCameraDriverSim.cpp @@ -44,8 +44,8 @@ void MultiCameraDriverSim::Initialize() m_hashKeySub = GetMainHashKey(); DBG_SIM_INFO("hash Key sub: %s", m_hashKeySub.c_str()); - auto pSimBridgeInfo = GetSimBridge(0); - auto pSimBridgeData = GetSimBridge(1); + auto pSimBridgeData = GetSimBridge(0); + auto pSimBridgeInfo = GetSimBridge(1); if (pSimBridgeInfo != nullptr) { @@ -64,11 +64,20 @@ void MultiCameraDriverSim::Initialize() const auto topic_base_name_ = multicamera_name_ + "/" + frame_id; DBG_SIM_INFO("topic_base_name:%s", topic_base_name_.c_str()); - pubImages.push_back(it.advertise(topic_base_name_ + "/image_raw", 1)); + sensor_msgs::msg::Image msg_img; + msg_img.header.frame_id = frame_id; - // Camera info publisher - auto camInfoPub = create_publisher(topic_base_name_ + "/camera_info", 1); - pubCamerasInfo.push_back(camInfoPub); + msg_imgs_[msg_imgs_.size()] = msg_img; + + pubImages_.push_back(it.advertiseCamera(topic_base_name_ + "/image_raw", 1)); + + // TODO: to supress the error log + // -> Failed to load plugin image_transport/compressed_pub, error string: parameter 'format' has already been declared + // -> Failed to load plugin image_transport/compressed_pub, error string: parameter 'png_level' has already been declared + // -> Failed to load plugin image_transport/compressed_pub, error string: parameter 'jpeg_quality' has already been declared + undeclare_parameter("format"); + undeclare_parameter("png_level"); + undeclare_parameter("jpeg_quality"); cameraInfoManager.push_back(std::make_shared(GetNode().get())); const auto camSensorMsg = GetCameraSensorMessage(pSimBridgeInfo, frame_id); @@ -80,7 +89,7 @@ void MultiCameraDriverSim::Initialize() void MultiCameraDriverSim::Deinitialize() { - for (auto pub : pubImages) + for (auto pub : pubImages_) { pub.shutdown(); } @@ -129,10 +138,9 @@ void MultiCameraDriverSim::UpdateData(const uint bridge_index) void *pBuffer = nullptr; int bufferLength = 0; - const bool succeeded = GetBufferFromSimulator(1, &pBuffer, bufferLength); + const bool succeeded = GetBufferFromSimulator(bridge_index, &pBuffer, bufferLength); if (!succeeded || bufferLength < 0) { - DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); return; } @@ -153,16 +161,15 @@ void MultiCameraDriverSim::UpdateData(const uint bridge_index) const uint32_t rows_arg = img->height(); const uint32_t step_arg = img->step(); - msg_img.header.stamp = m_simTime; - sensor_msgs::fillImage(msg_img, encoding_arg, rows_arg, cols_arg, step_arg, + auto const msg_img = &msg_imgs_[i]; + msg_img->header.stamp = m_simTime; + sensor_msgs::fillImage(*msg_img, encoding_arg, rows_arg, cols_arg, step_arg, reinterpret_cast(img->data().data())); - pubImages.at(i).publish(msg_img); - // Publish camera info auto camera_info_msg = cameraInfoManager[i]->getCameraInfo(); camera_info_msg.header.stamp = m_simTime; - pubCamerasInfo[i]->publish(camera_info_msg); + pubImages_.at(i).publish(*msg_img, camera_info_msg); } } \ No newline at end of file diff --git a/realsense_driver_sim/include/realsense_driver_sim/RealSenseDriverSim.hpp b/realsense_driver_sim/include/realsense_driver_sim/RealSenseDriverSim.hpp index 15c6c05a..f57cf952 100644 --- a/realsense_driver_sim/include/realsense_driver_sim/RealSenseDriverSim.hpp +++ b/realsense_driver_sim/include/realsense_driver_sim/RealSenseDriverSim.hpp @@ -53,14 +53,14 @@ class RealSenseDriverSim : public DriverSim // message for ROS2 communictaion std::map msg_imgs_; - // Camera info publishers. - std::map::SharedPtr> pubCameraInfos_; + // // Camera info publishers. + // std::map::SharedPtr> pubCameraInfos_; // Camera info managers std::map> cameraInfoManager_; // Image publisher - std::map pubImages_; + std::map pubImages_; }; #endif \ No newline at end of file diff --git a/realsense_driver_sim/src/RealSenseDriverSim.cpp b/realsense_driver_sim/src/RealSenseDriverSim.cpp index 1a1852fe..97fc1c00 100644 --- a/realsense_driver_sim/src/RealSenseDriverSim.cpp +++ b/realsense_driver_sim/src/RealSenseDriverSim.cpp @@ -77,7 +77,7 @@ void RealSenseDriverSim::Initialize() hashKeySubs_[simBridgeCount] = hashKeySub; const auto topic_name = (module.find("depth") == string::npos)? "image_raw":"image_rect_raw"; - pubImages_[simBridgeCount] = it.advertise(topic_base_name_ + "/" + topic_name, 1); + pubImages_[simBridgeCount] = it.advertiseCamera(topic_base_name_ + "/" + topic_name, 1); // TODO: to supress the error log // -> Failed to load plugin image_transport/compressed_pub, error string: parameter 'format' has already been declared @@ -87,7 +87,7 @@ void RealSenseDriverSim::Initialize() undeclare_parameter("png_level"); undeclare_parameter("jpeg_quality"); - pubCameraInfos_[simBridgeCount] = create_publisher(topic_base_name_ + "/camera_info", 1); + // pubCameraInfos_[simBridgeCount] = create_publisher(topic_base_name_ + "/camera_info", 1); sensor_msgs::msg::Image msg_img; msg_img.header.frame_id = module; @@ -255,7 +255,6 @@ void RealSenseDriverSim::UpdateData(const uint bridge_index) const bool succeeded = GetBufferFromSimulator(bridge_index, &pBuffer, bufferLength); if (!succeeded || bufferLength < 0) { - DBG_SIM_ERR("zmq receive error return size(%d): %s", bufferLength, zmq_strerror(zmq_errno())); return; } @@ -322,11 +321,9 @@ void RealSenseDriverSim::UpdateData(const uint bridge_index) } } - pubImages_[bridge_index].publish(*msg_img); - // Publish camera info auto camera_info_msg = cameraInfoManager_[bridge_index]->getCameraInfo(); camera_info_msg.header.stamp = m_simTime; - pubCameraInfos_[bridge_index]->publish(camera_info_msg); + pubImages_[bridge_index].publish(*msg_img, camera_info_msg); } diff --git a/sim_bridge/include/sim_bridge/sim_bridge.hpp b/sim_bridge/include/sim_bridge/sim_bridge.hpp index c5acc5e9..d697bd65 100644 --- a/sim_bridge/include/sim_bridge/sim_bridge.hpp +++ b/sim_bridge/include/sim_bridge/sim_bridge.hpp @@ -59,7 +59,8 @@ class SimBridge const uint8_t tagSize = 8; // The size of zmq packet header tag const int keepOnlyLastMsg = 1; - const int reconnect_ivl_min = 500; + const int reconnect_ivl_min = 1000; + const int reconnect_ivl_max = 5000; const int lingerPeriod = 0; const int recv_timeout = 2000; // milliseconds diff --git a/sim_bridge/package.xml b/sim_bridge/package.xml index 2d6d1af5..4afe11cf 100644 --- a/sim_bridge/package.xml +++ b/sim_bridge/package.xml @@ -9,6 +9,8 @@ ament_cmake + libzmq3-dev + ament_cmake diff --git a/sim_bridge/src/sim_bridge.cpp b/sim_bridge/src/sim_bridge.cpp index 97fc9778..bff6aace 100644 --- a/sim_bridge/src/sim_bridge.cpp +++ b/sim_bridge/src/sim_bridge.cpp @@ -104,6 +104,12 @@ bool SimBridge::SetupCommon(void* const targetSocket) return false; } + if (zmq_setsockopt(targetSocket, 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))) { lastErrMsg = "SetSock Err:" + string(zmq_strerror(zmq_errno())); @@ -429,7 +435,7 @@ bool SimBridge::Receive(void** buffer, int& bufferLength, bool isNonBlockingMode if ((bufferLength = zmq_msg_recv(&m_msgRx, pSockRx_, (isNonBlockingMode) ? ZMQ_DONTWAIT : 0)) < 0) { - DBG_SIM_ERR("failed to receive ZMQ message: %s", zmq_strerror(zmq_errno())); + DBG_SIM_ERR("failed to receive ZMQ message: err(%s) length(%d)", zmq_strerror(zmq_errno()), bufferLength); return false; } diff --git a/unity_ros/CMakeLists.txt b/unity_ros/CMakeLists.txt index eadae708..671890d6 100644 --- a/unity_ros/CMakeLists.txt +++ b/unity_ros/CMakeLists.txt @@ -35,7 +35,7 @@ include_directories(include) add_executable(${PROJECT_NAME} src/main.cpp - src/unity_ros_init.cpp) + src/unity_ros.cpp) ament_target_dependencies(${PROJECT_NAME} ${dependencies} diff --git a/unity_ros/include/unity_ros/unity_ros_init.hpp b/unity_ros/include/unity_ros/unity_ros.hpp similarity index 95% rename from unity_ros/include/unity_ros/unity_ros_init.hpp rename to unity_ros/include/unity_ros/unity_ros.hpp index eb6d03fd..9c23fa86 100644 --- a/unity_ros/include/unity_ros/unity_ros_init.hpp +++ b/unity_ros/include/unity_ros/unity_ros.hpp @@ -1,5 +1,5 @@ /** - * @file unity_ros_init.hpp + * @file unity_ros.hpp * @date 2020-04-08 * @author Hyunseok Yang * @brief @@ -14,8 +14,8 @@ * SPDX-License-Identifier: MIT */ -#ifndef _UnityRosInit_H_ -#define _UnityRosInit_H_ +#ifndef _UnityRos_HPP_ +#define _UnityRos_HPP_ #include #include diff --git a/unity_ros/src/main.cpp b/unity_ros/src/main.cpp index ca7ca07e..4b4ef2ff 100644 --- a/unity_ros/src/main.cpp +++ b/unity_ros/src/main.cpp @@ -12,7 +12,7 @@ * * SPDX-License-Identifier: MIT */ -#include "unity_ros/unity_ros_init.hpp" +#include "unity_ros/unity_ros.hpp" using namespace std::chrono_literals; diff --git a/unity_ros/src/unity_ros_init.cpp b/unity_ros/src/unity_ros.cpp similarity index 97% rename from unity_ros/src/unity_ros_init.cpp rename to unity_ros/src/unity_ros.cpp index 9d047870..099c0095 100644 --- a/unity_ros/src/unity_ros_init.cpp +++ b/unity_ros/src/unity_ros.cpp @@ -1,5 +1,5 @@ /** - * @file unity_ros_init.cpp + * @file unity_ros.cpp * @date 2020-04-08 * @author Hyunseok Yang * @brief @@ -14,7 +14,7 @@ * SPDX-License-Identifier: MIT */ -#include "unity_ros/unity_ros_init.hpp" +#include "unity_ros/unity_ros.hpp" #include #include #include @@ -26,7 +26,7 @@ using namespace UnityRos; using namespace gazebo; UnityRosInit::UnityRosInit() - : Node("unity_ros_init", + : Node("unity_ros", rclcpp::NodeOptions() .parameter_overrides(vector{rclcpp::Parameter("use_sim_time", true)}) .allow_undeclared_parameters(true)