Merge branch 'master' into feat/ekf

.gitmodules

@@ -21,3 +21,7 @@
 	path = external/articubot_one
 	url = https://github.com/joshnewans/articubot_one.git
 	branch = humble
+[submodule "external/vectornav"]
+	path = external/vectornav
+	url = https://github.com/dawonn/vectornav.git
+	branch = ros2

documents/installation/vectornav_imu.md

@@ -0,0 +1,62 @@
+# Launch Instructions for VectorNav with Permission Issues
+
+This document explains how to resolve permission issues for serial devices (like `/dev/ttyUSB0`) and launch the VectorNav package on Linux.
+
+## Prerequisites
+
+1. **Install Necessary Packages**: Ensure all required packages are installed in your workspace and that the workspace is correctly sourced.
+
+2. **Device Permissions**: You’ll need permission to access the device files (e.g., `/dev/ttyUSB0`).
+
+## Steps
+
+### Step 1: Check Device Permissions
+
+1. First, identify the device port:
+   ```bash
+   ls /dev/ttyUSB*
+   ```
+2. Verify your user has permission to access the device. If you see an error or permission denied, proceed with the steps below.
+
+### Step 2: Add User to the `dialout` Group
+
+The `dialout` group typically has access to serial devices. Add your user to this group:
+
+```bash
+sudo usermod -aG dialout $USER
+```
+
+**Note**: Log out and log back in for the group change to take effect. Alternatively, you can restart the system to apply these changes immediately.
+
+### Step 3: Verify Permissions
+
+After re-logging or rebooting, check that you have read and write permissions on the device:
+
+```bash
+ls -l /dev/ttyUSB0
+```
+
+If permissions are set correctly, the output should show that `dialout` has `rw` (read/write) access for `/dev/ttyUSB0`.
+
+### Step 4: Launch VectorNav
+
+Source your ROS workspace, then launch the VectorNav nodes with the appropriate configurations:
+
+```bash
+source ~/Documents/urc/rover-colcon/install/setup.bash
+ros2 launch vectornav vectornav.launch.py
+```
+
+**Note**: Replace the workspace path with the correct path if it’s different on your system.
+
+### Troubleshooting
+
+If you continue to experience issues with permissions, consider checking `dmesg` logs to ensure the device is recognized:
+
+```bash
+dmesg | grep ttyUSB
+```
+
+This command shows messages related to the USB connection and may help diagnose connectivity issues.
+
+--- 

urc_bringup/config/controller_config.yaml

@@ -125,7 +125,7 @@ joy_drive:
     max_linear_velocity_ms: 3.0
     max_angular_velocity_radians: 5.0
     joy_command_topic: /driver/joy
-    cmd_vel_topic: /rover_drivetrain_controller/cmd_vel
+    cmd_vel_topic: /cmd_vel_teleop
     target_axes: [1, 3]
     invert_linear_velocity: false
     invert_angular_velocity: false

urc_bringup/launch/bringup.launch.py

@@ -27,10 +27,14 @@ def generate_launch_description():
     pkg_nmea_navsat_driver = FindPackageShare("nmea_navsat_driver").find(
         "nmea_navsat_driver"
     )
+    pkg_urc_platform = get_package_share_directory("urc_platform")
+
+    pkg_vectornav = get_package_share_directory("vectornav")
 
     controller_config_file_dir = os.path.join(
         pkg_urc_bringup, "config", "controller_config.yaml"
     )
+    twist_mux_config = os.path.join(pkg_urc_platform, "config", "twist_mux.yaml")
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
 
     xacro_file = os.path.join(
@@ -41,15 +45,15 @@ def generate_launch_description():
         xacro_file, mappings={"use_simulation": "false"}
     )
     robot_desc = robot_description_config.toxml()
-    gps_config = os.path.join(get_package_share_directory("urc_bringup"),
-                              "config", "nmea_serial_driver.yaml")
+    gps_config = os.path.join(
+        get_package_share_directory("urc_bringup"), "config", "nmea_serial_driver.yaml"
+    )
 
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[controller_config_file_dir,
-                    {"robot_description": robot_desc}],
-        output="both"
+        parameters=[controller_config_file_dir, {"robot_description": robot_desc}],
+        output="both",
     )
 
     load_robot_state_publisher = Node(
@@ -65,7 +69,7 @@ def generate_launch_description():
     load_joint_state_broadcaster = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["-p", controller_config_file_dir, "joint_state_broadcaster"],
+        arguments=["-p", controller_config_file_dir, "joint_state_broadcaster"]
     )
 
     load_drivetrain_controller = Node(
@@ -77,37 +81,20 @@ def generate_launch_description():
     load_status_light_controller = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["-p", controller_config_file_dir, "status_light_controller"],
+        arguments=["-p", controller_config_file_dir, "status_light_controller"]
     )
 
-    # load_arm_controller = Node(
-    #     package="controller_manager",
-    #     executable="spawner",
-    #     arguments=["-p", controller_config_file_dir, "arm_controller"],
-    # )
-
-    # load_gripper_controller_left = Node(
-    #     package="controller_manager",
-    #     executable="spawner",
-    #     arguments=["-p", controller_config_file_dir, "gripper_controller_left"],
-    # )
-
-    # load_gripper_controller_right = Node(
-    #     package="controller_manager",
-    #     executable="spawner",
-    #     arguments=["-p", controller_config_file_dir, "gripper_controller_right"],
-    # )
-
-    teleop_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [FindPackageShare("urc_bringup"), "/launch/teleop.launch.py"]
-        )
+    twist_mux_node = Node(
+        package="urc_platform",
+        executable="urc_platform_TwistMux",
+        name="twist_mux",
     )
 
     launch_gps = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(
-                pkg_nmea_navsat_driver, "launch", "nmea_serial_driver.launch.py"
+                pkg_nmea_navsat_driver,
+                "launch", "nmea_serial_driver.launch.py"
             )
         )
     )
@@ -118,15 +105,31 @@ def generate_launch_description():
         output='screen',
         parameters=[gps_config])
 
+    launch_vectornav = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_vectornav, "launch", "vectornav.launch.py")
+        )
+    )
+
     rosbridge_server_node = Node(
         package="rosbridge_server",
         name="rosbridge_server",
         executable="rosbridge_websocket.py",
         parameters=[{"port": 9090}],
     )
 
+    twist_mux_node = Node(
+        package="twist_mux",
+        executable="twist_mux",
+        name="twist_mux",
+        output="screen",
+        parameters=[twist_mux_config],
+        remappings=[("/cmd_vel_out", "/rover_drivetrain_controller/cmd_vel")],
+    )
+
     odom_frame_node = Node(
-        package="urc_tf", executable="urc_tf_WorldFrameBroadcaster", output="screen"
+        package="urc_tf", executable="urc_tf_WorldFrameBroadcaster",
+        output="screen"
     )
 
     return LaunchDescription(
@@ -146,12 +149,10 @@ def generate_launch_description():
             load_joint_state_broadcaster,
             load_drivetrain_controller,
             load_status_light_controller,
-            # load_arm_controller,
-            # load_gripper_controller_left,
-            # load_gripper_controller_right,
-            teleop_launch,
+            twist_mux_node,
             launch_gps,
             rosbridge_server_node,
-            odom_frame_node
+            odom_frame_node,
+            launch_vectornav,
         ]
     )

urc_bringup/launch/bringup_simulation.launch.py

@@ -26,6 +26,7 @@ def generate_launch_description():
     controller_config_file_dir = os.path.join(
         pkg_urc_bringup, "config", "ros2_control_walli.yaml"
     )
+
     # world_path = os.path.join(pkg_urc_gazebo, "urdf/worlds/urc_world.world")
     use_sim_time = LaunchConfiguration("use_sim_time", default="true")
 
@@ -118,6 +119,19 @@ def generate_launch_description():
         )
     )
 
+    twist_mux_node = Node(
+        package="urc_platform",
+        executable="urc_platform_TwistMux",
+        name="twist_mux",
+    )
+
+    # ekf_launch = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         [FindPackageShare("urc_localization"),
+    #          "/launch/dual_ekf_navsat.launch.py"]
+    #     )
+    # )
+
     bt_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(pkg_urc_bringup, "launch", "bt.launch.py")
@@ -180,7 +194,7 @@ def generate_launch_description():
             RegisterEventHandler(
                 event_handler=OnProcessExit(
                     target_action=load_drivetrain_controller,
-                    on_exit=[elevation_mapping_node],
+                    on_exit=[elevation_mapping_node, twist_mux_node],
                 )
             ),
             RegisterEventHandler(

urc_bringup/package.xml

@@ -20,4 +20,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

urc_hw/src/hardware_interfaces/status_light.cpp

@@ -118,12 +118,6 @@ hardware_interface::CallbackReturn StatusLight::on_deactivate(const rclcpp_lifec
 
 hardware_interface::return_type StatusLight::read(const rclcpp::Time &, const rclcpp::Duration &)
 {
-  RCLCPP_INFO(
-    rclcpp::get_logger(
-      hardware_interface_name), "Red: %s, Green: %s, Blue: %s", num_to_state(
-      lightStates[0]).c_str(), num_to_state(lightStates[1]).c_str(), num_to_state(
-      lightStates[2]).c_str());
-
   return hardware_interface::return_type::OK;
 }
 

urc_navigation/trajectory_following/config/pure_pursuit_config.yaml

@@ -4,7 +4,7 @@ follower_action_server:
     desired_linear_velocity: 0.5
     lethal_cost_threshold: 10
     cmd_vel_stamped: true
-    cmd_vel_topic: "/rover_drivetrain_controller/cmd_vel"
+    cmd_vel_topic: "/cmd_vel_autonomous"
     odom_topic: "/rover_drivetrain_controller/odom"
     map_frame: "map"
     goal_tolerance: 0.1

urc_platform/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 find_package(usb_cam REQUIRED)
+find_package(vectornav_msgs REQUIRED)
 
 include_directories(
   include
@@ -19,6 +20,7 @@ include_directories(
 # Library creation
 add_library(${PROJECT_NAME} SHARED
   src/joystick_driver.cpp
+  src/twist_mux.cpp
 )
 
 set(dependencies
@@ -28,6 +30,7 @@ set(dependencies
   sensor_msgs
   diagnostic_updater
   usb_cam
+  vectornav_msgs
 )
 
 ament_target_dependencies(${PROJECT_NAME}
@@ -41,6 +44,12 @@ rclcpp_components_register_node(
   EXECUTABLE ${PROJECT_NAME}_JoystickDriver
 )
 
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "twist_mux::TwistMux"
+  EXECUTABLE ${PROJECT_NAME}_TwistMux
+)
+
 # Install launch files.
 install(
   DIRECTORY
@@ -76,4 +85,4 @@ ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})
 ament_export_dependencies(${dependencies})
 
-ament_package()
+ament_package()

urc_platform/config/twist_mux.yaml

@@ -0,0 +1,21 @@
+twist_mux:
+  ros__parameters:
+    topics:
+      autonomous_navigation:
+        topic: /cmd_vel_autonomous
+        timeout: 10.0
+        priority: 1
+      teleoperation:
+        topic: /cmd_vel_teleop
+        timeout: 0.5
+        priority: 3
+
+    locks:
+      autonomous_navigation:
+        topic: /autonomous_disabled
+        timeout: 0.0
+        priority: 2
+      e_stop:
+        topic: /e_stop
+        timeout: 0.0
+        priority: 255

urc_platform/include/twist_mux.hpp

@@ -0,0 +1,36 @@
+#ifndef TWIST_MUX
+#define TWIST_MUX
+
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/string.hpp"
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+
+namespace twist_mux
+{
+class TwistMux : public rclcpp::Node
+{
+public:
+  explicit TwistMux(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr
+    autonomous_subscriber;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr
+    teleop_subscriber;
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr enabled_subscriber;
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr mode_subscriber;
+
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>>
+  cmd_publisher;
+
+  bool enabled;
+  std::string mode;
+};
+} // namespace twist_mux
+
+#endif

urc_platform/package.xml

@@ -21,6 +21,7 @@
   <depend>usb_cam</depend>
   <depend>image_transport</depend>
   <depend>image_transport_plugins</depend>
+  <depend>vectornav_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>