[sensors] Launch vectornav in bringup.launch.py

.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/launch/bringup.launch.py

@@ -29,6 +29,10 @@ def generate_launch_description():
     )
     pkg_imu_driver = FindPackageShare("imu_driver").find("imu_driver")
 
+    pkg_vectornav = get_package_share_directory("vectornav")
+
+
+
     controller_config_file_dir = os.path.join(
         pkg_urc_bringup, "config", "controller_config.yaml"
     )
@@ -81,6 +85,13 @@ def generate_launch_description():
         arguments=["-p", controller_config_file_dir, "status_light_controller"],
     )
 
+
+    imu_parser_node = Node(
+        package="urc_platform",
+        executable="urc_platform_IMUParser"
+    )
+
+
     # load_arm_controller = Node(
     #     package="controller_manager",
     #     executable="spawner",
@@ -125,6 +136,13 @@ def generate_launch_description():
         )
     )
 
+    # change here
+    launch_vectornav = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_vectornav, "launch", "vectornav.launch.py")
+        )
+    )
+
     rosbridge_server_node = Node(
         package="rosbridge_server",
         name="rosbridge_server",
@@ -160,6 +178,8 @@ def generate_launch_description():
             launch_gps,
             launch_imu,
             rosbridge_server_node,
-            odom_frame_node
+            odom_frame_node,
+            launch_vectornav,
+            imu_parser_node
         ]
     )

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/imu_parser.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 "imu_parser::IMUParser"
+  EXECUTABLE ${PROJECT_NAME}_IMUParser
+)
+
 # Install launch files.
 install(
   DIRECTORY

urc_platform/include/imu_parser.hpp

@@ -0,0 +1,33 @@
+#ifndef IMU_PARSER_H
+#define IMU_PARSER_H
+
+#include <bits/stdc++.h>
+#include <math.h>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <unistd.h>
+#include <vectornav_msgs/msg/common_group.hpp>
+
+namespace imu_parser
+{
+
+class IMUParser : public rclcpp::Node
+{
+public:
+  explicit IMUParser(const rclcpp::NodeOptions & options);
+
+private:
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr
+    imu_publisher;
+
+  rclcpp::Subscription<vectornav_msgs::msg::CommonGroup>::SharedPtr vectornav_subscriber;
+
+  void VectornavCallback(const vectornav_msgs::msg::CommonGroup & msg);
+
+};
+
+} // namespace imu_parser
+
+#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>

urc_platform/src/imu_parser.cpp

@@ -0,0 +1,57 @@
+#include "imu_parser.hpp"
+
+namespace imu_parser
+{
+
+int header = 0;
+IMUParser::IMUParser(const rclcpp::NodeOptions & options)
+: rclcpp::Node("imu_parser", options)
+{
+    vectornav_subscriber = create_subscription<vectornav_msgs::msg::CommonGroup>(
+    "/vectornav/raw/common", rclcpp::SystemDefaultsQoS(),
+    [this](const vectornav_msgs::msg::CommonGroup msg) {VectornavCallback(msg);});
+
+    imu_publisher = create_publisher<sensor_msgs::msg::Imu>(
+    "/imu/data", rclcpp::SystemDefaultsQoS());
+}
+
+void IMUParser::VectornavCallback(const vectornav_msgs::msg::CommonGroup & msg)
+{
+    sensor_msgs::msg::Imu result;
+    result.header.frame_id = std::to_string(header++);
+
+    result.orientation.x = msg.quaternion.x;
+    result.orientation.y = msg.quaternion.y;
+    result.orientation.z = msg.quaternion.z;
+    result.orientation.w = msg.quaternion.w;
+    result.orientation_covariance = {
+        0.01,  0.0,   0.0,
+        0.0,   0.01,  0.0,
+        0.0,   0.0,   0.01
+    };
+
+    result.angular_velocity.x = msg.angularrate.x;
+    result.angular_velocity.y = msg.angularrate.y;
+    result.angular_velocity.z = msg.angularrate.z;
+    result.angular_velocity_covariance = {
+        0.01,  0.0,   0.0,
+        0.0,   0.01,  0.0,
+        0.0,   0.0,   0.01
+    };
+
+    result.linear_acceleration.x = msg.accel.x;
+    result.linear_acceleration.y = msg.accel.y;
+    result.linear_acceleration.z = msg.accel.z;
+    result.linear_acceleration_covariance = {
+        0.01,  0.0,   0.0,
+        0.0,   0.01,  0.0,
+        0.0,   0.0,   0.01
+    };
+
+
+    imu_publisher->publish(result);
+}
+
+} // namespace orchestrator
+
+RCLCPP_COMPONENTS_REGISTER_NODE(imu_parser::IMUParser)