Add CARLA waypoint publisher to ROS bridge (#164)

* Add waypoint publisher

* change topic name

* Fix code style issues with Autopep8

* Fix code style issues with clang_format

---------

Co-authored-by: WATonomousAdmin <[email protected]>

docker/simulation/carla_ros_bridge/carla_ros_bridge.Dockerfile

@@ -80,6 +80,7 @@ ARG CARLA_VERSION
 #Install Python Carla API
 COPY --from=wato_carla_api --chown=root /home/carla/PythonAPI/carla /opt/carla/PythonAPI
 WORKDIR /opt/carla/PythonAPI
+RUN cp -r ./agents /usr/local/lib/python3.8/dist-packages/
 RUN python3.8 -m easy_install pip && \
       pip3 install carla==${CARLA_VERSION} && \
       pip install simple-pid==2.0.0 && \

src/simulation/carla_config/CMakeLists.txt

@@ -20,9 +20,11 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(path_planning_msgs REQUIRED)
 find_package(embedded_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 
 # Create ROS2 node executable from source files
 add_executable(carla_mpc_bridge src/carla_mpc_bridge.cpp)
+add_executable(carla_waypoint_modifier src/carla_waypoint_fix.cpp)
 
 if(BUILD_TESTING)
   # Search for dependencies required for building tests + linting
@@ -48,10 +50,12 @@ endif()
 
 # Link with dependencies
 ament_target_dependencies(carla_mpc_bridge rclcpp path_planning_msgs embedded_msgs)
+ament_target_dependencies(carla_waypoint_modifier rclcpp nav_msgs)
 
 # Copy executable to installation location
 install(TARGETS
   carla_mpc_bridge
+  carla_waypoint_modifier
   DESTINATION lib/${PROJECT_NAME})
 
 # Copy launch and config files to installation location
@@ -60,4 +64,4 @@ install(DIRECTORY
   launch
   DESTINATION share/${PROJECT_NAME})
 
-ament_package()
+ament_package()

src/simulation/carla_config/launch/carla.launch.py

@@ -150,6 +150,39 @@ def generate_launch_description():
         output='screen'
     )
 
+    waypoint_topic = DeclareLaunchArgument('waypoint_topic', default_value=[
+                                           '/carla/', LaunchConfiguration('role_name'), '/waypoints'])
+    waypoint_topic_old = DeclareLaunchArgument('waypoint_topic_old', default_value=[
+                                               '/carla/', LaunchConfiguration('role_name'), '/waypointsOld'])
+
+    """ Launch CARLA Waypoint Publisher """
+    carla_waypoint_publisher = Node(
+        package='carla_waypoint_publisher',
+        executable='carla_waypoint_publisher',
+        name='carla_waypoint_publisher',
+        output='screen',
+        parameters=[{
+            'host': LaunchConfiguration('host'),
+            'port': LaunchConfiguration('port'),
+            'timeout': LaunchConfiguration('timeout'),
+            'role_name': LaunchConfiguration('role_name')
+        }],
+        remappings=[
+            (LaunchConfiguration('waypoint_topic'), LaunchConfiguration('waypoint_topic_old')),
+        ]
+    )
+
+    """ Launch Waypoint Modifier Node """
+    carla_waypoint_modifier = Node(
+        package='carla_config',
+        executable='carla_waypoint_modifier',
+        parameters=[{
+            'input_topic': LaunchConfiguration('waypoint_topic_old'),
+            'output_topic': LaunchConfiguration('waypoint_topic')
+        }],
+        output='screen'
+    )
+
     return LaunchDescription([
         host_arg,
         port_arg,
@@ -166,5 +199,9 @@ def generate_launch_description():
         carla_ros_bridge,
         carla_ego_vehicle,
         *carla_control,
+        waypoint_topic_old,
+        waypoint_topic,
+        carla_waypoint_publisher,
+        carla_waypoint_modifier,
         # carla_mpc_bridge, # MPC bridge needs to be reworked
     ])

src/simulation/carla_config/package.xml

@@ -11,10 +11,11 @@
   <depend>rclcpp</depend>
   <depend>path_planning_msgs</depend>
   <depend>embedded_msgs</depend>
+  <depend>nav_msgs</depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

src/simulation/carla_config/src/carla_waypoint_fix.cpp

@@ -0,0 +1,62 @@
+/*
+
+Node to add frame IDs to the waypoints being published by carla_waypoint_publisher
+
+*/
+
+#include "nav_msgs/msg/path.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class Waypoint_Modifier_Node : public rclcpp::Node {
+ public:
+  Waypoint_Modifier_Node() : Node("carla_waypoint_fix") {
+    // Declare Parameters
+    this->declare_parameter("input_topic", "/carla/ego/waypointsOld");
+    this->declare_parameter("output_topic", "/carla/ego/waypoints");
+
+    // Get Parameters
+    std::string input_topic = this->get_parameter("input_topic").as_string();
+    std::string output_topic = this->get_parameter("output_topic").as_string();
+
+    // Subscribe to path
+    pathSub = this->create_subscription<nav_msgs::msg::Path>(
+        input_topic, 1,
+        std::bind(&Waypoint_Modifier_Node::publish_path, this, std::placeholders::_1));
+
+    // Create Publisher
+    auto qos = rclcpp::QoS(rclcpp::KeepAll())
+                   .reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE)
+                   .durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+    this->pathPub = this->create_publisher<nav_msgs::msg::Path>(output_topic, qos);
+  }
+
+ private:
+  void publish_path(nav_msgs::msg::Path::SharedPtr pathIn) {
+    std::string frame_id = pathIn->header.frame_id;
+    path.header = pathIn->header;
+    path.poses = pathIn->poses;
+    for (auto &pose : path.poses) {
+      pose.header.frame_id = frame_id;
+    }
+    pathPub->publish(path);
+  }
+
+  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr pathSub;
+  rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pathPub;
+
+  nav_msgs::msg::Path path;
+};
+
+int main(int argc, char **argv) {
+  // Initialize ROS2
+  rclcpp::init(argc, argv);
+
+  // Create Node
+  auto node = std::make_shared<Waypoint_Modifier_Node>();
+
+  rclcpp::spin(node);
+
+  rclcpp::shutdown();
+
+  return 0;
+}