World modeling robot localization launch file

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 && \

docker/world_modeling/localization/localization.Dockerfile

@@ -7,7 +7,6 @@ WORKDIR ${AMENT_WS}/src
 
 # Copy in source code 
 COPY src/world_modeling/localization localization
-COPY src/wato_msgs/sample_msgs sample_msgs
 
 # Scan for rosdeps
 RUN apt-get -qq update && rosdep update && \
@@ -21,7 +20,7 @@ FROM ${BASE_IMAGE} as dependencies
 
 # Install Rosdep requirements
 COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list
-RUN apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
+RUN apt-get -qq update && apt-fast install -qq -y --no-install-recommends $(cat /tmp/colcon_install_list)
 
 # Copy in source code from source stage
 WORKDIR ${AMENT_WS}

modules/dev_overrides/docker-compose.simulation.yaml

@@ -15,10 +15,10 @@ services:
     extends:
       file: ../docker-compose.simulation.yaml
       service: carla_ros_bridge
-    command: tail -F anything
+    # command: tail -F anything
     volumes:
       - ${MONO_DIR}/src/simulation/carla_config:/home/bolty/ament_ws/src/carla_config
-    # command: /bin/bash -c "ros2 launch carla_config carla.launch.py"
+    command: /bin/bash -c "ros2 launch carla_config carla.launch.py"
 
   carla_viz:
     extends:

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

src/world_modeling/localization/CMakeLists.txt

@@ -9,6 +9,12 @@ endif()
 find_package(ament_cmake REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(robot_localization REQUIRED)
+
+# Copy launch and config files to installation location
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME})
 
 ament_package()

src/world_modeling/localization/config/config.yaml

@@ -0,0 +1,46 @@
+ekf_filter_node_global:
+  ros__parameters:
+    # Frame definitions
+    # map_frame: map # Fixed global frame, global navigation
+    odom_frame: map # Local reference that tracks robot position over time based on wheel encoders
+    base_link_frame: ego # Robot's body frame, centered on the robot and moves with it
+    world_frame: map
+    debug: true
+
+    odom0: /carla/ego/odometry  # Provides information about robot's position and velocity
+    odom0_config: [true, true, false, 
+            false, false, false, 
+            false, false, false, 
+            false, false, false, 
+            false, false, false]  # 2D configuration for X, Y position, velocity, and YAW
+
+    imu0: /carla/ego/imu		# Inertial measurement unit, provides info on orientation, accelerations, angular velocity
+    imu0_config: [false, false, false, 
+            false, false, true, 
+            false, false, false, 
+            false, false, false, 
+            false, false, false]  # Yaw is important for steering and orientation
+
+    # Parameters
+    frequency: 30.0
+    sensor_timeout: 0.1
+    two_d_mode: true
+    publish_tf: false
+
+navsat_transform:
+  ros__parameters:
+    frequency: 30.0
+    delay: 3.0
+    magnetic_declination_radians: 0.0
+    yaw_offset: 0.0
+    zero_altitude: true
+    broadcast_utm_transform: true
+    publish_filtered_gps: true
+    use_odometry_yaw: true
+    wait_for_datum: false 
+
+# #GPS
+# GPS:
+#   port: /dev/ttyUSB0
+#     baud: 19200
+

src/world_modeling/localization/launch/localization.launch.py

@@ -0,0 +1,49 @@
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+import os
+
+
+def generate_launch_description():
+    """Launch localization node."""
+    config_file_path = os.path.join(
+        get_package_share_directory('localization'),
+        'config',
+        'config.yaml'
+    )
+
+
+    return LaunchDescription([
+        # Declare the launch argument
+        DeclareLaunchArgument(
+            'config',
+            default_value = config_file_path,
+        ),
+
+        Node(
+            package='robot_localization',
+            executable='ekf_node',
+            name='ekf_filter_node_global',
+            parameters=[config_file_path],
+            remappings=[("odometry/filtered", "odometry/filtered")],
+            output='screen'
+        ),
+        Node(
+            package='robot_localization',
+            executable='navsat_transform_node',
+            name='navsat_transform',
+            parameters=[config_file_path],
+            output='screen',
+            remappings=[
+                ("imu/data", "carla/ego/imu"),
+                ("gps/fix", "carla/ego/gnss"),
+                ("gps/filtered", "gps/filtered"),
+                ("odometry/gps", "odometry/gps"),
+                ("odometry/filtered", "odometry/filtered"),
+            ],
+        )
+    ])
+

src/world_modeling/localization/package.xml

@@ -3,11 +3,12 @@
 <package format="3">
   <name>localization</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="e23zhou@uwaterloo.ca">bolty</maintainer>
+  <description>Localization using robot_localization package</description>
+  <maintainer email="v3jayaku@watonomous.ca">Vishal Jayakumar</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>robot_localization</depend>
 
 
   <export>