Merge pull request #8 from lge-ros2/main

Merge 'main' branch into 'foxy'

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"]

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

bringup/package.xml

@@ -33,6 +33,8 @@
   <test_depend>launch</test_depend>
   <test_depend>launch_testing</test_depend>
 
+  <depend>python3-websocket</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

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<sensor_msgs::msg::CameraInfo>::SharedPtr pubCameraInfo{nullptr};
+  image_transport::CameraPublisher pubImage;
 
   // Camera info manager
   std::shared_ptr<camera_info_manager::CameraInfoManager> cameraInfoManager;

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<sensor_msgs::msg::CameraInfo>(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<const void *>(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);
 }

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

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);

driver_sim/package.xml

@@ -21,6 +21,8 @@
   <exec_depend>driver_sim_interfaces</exec_depend>
   <exec_depend>sim_bridge</exec_depend>
 
+  <depend>protobuf-dev</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>

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;
   }
 

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 

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

elevator_system_sim/package.xml

@@ -12,13 +12,13 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>rclcpp</depend>
-  <depend>rclcpp_lifecycle</depend>
-  <depend>sensor_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>elevator_system_msgs</depend>
-  <depend>driver_sim</depend>
-  <depend>camera_driver_sim</depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_lifecycle</build_depend>
+  <build_depend>driver_sim_interfaces</build_depend>
+  <build_depend>elevator_system_msgs</build_depend>
+  <build_depend>sim_bridge</build_depend>
+
+  <exec_depend>driver_sim_interfaces</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

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>{rclcpp::Parameter("use_sim_time", true)})
                         .automatically_declare_parameters_from_overrides(true)
                         .use_intra_process_comms(intra_process_comms))
     , m_pSimBridge(new SimBridge())

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;
   }
 

lidar_driver_sim/package.xml

@@ -12,7 +12,6 @@
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
-  <depend>image_transport</depend>
   <depend>driver_sim</depend>
 
   <export>

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;
   }
 

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;
   }
 

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<rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> pubCamerasInfo;
+  std::map<int, sensor_msgs::msg::Image> msg_imgs_;
 
   // Camera info managers
   std::vector<std::shared_ptr<camera_info_manager::CameraInfoManager>> cameraInfoManager;
 
   // Image publisher
-  std::vector<image_transport::Publisher> pubImages;
+  std::vector<image_transport::CameraPublisher> pubImages_;
 };
 #endif