Get Hardware working

c300/c300_bringup/config/urg_node_ethernet.yaml

@@ -0,0 +1,18 @@
+urg_node:
+  ros__parameters:
+    angle_max: 3.14
+    angle_min: -3.14
+    ip_address: "192.168.0.10"
+    ip_port: 10940
+    serial_baud: 115200
+    laser_frame_id: "base_laser"
+    calibrate_time: false
+    default_user_latency: 0.0
+    diagnostics_tolerance: 0.05
+    diagnostics_window_time: 5.0
+    error_limit: 4
+    get_detailed_status: false
+    publish_intensity: false
+    publish_multiecho: false
+    cluster: 1
+    skip: 0

c300/c300_bringup/launch/hardware_c300_launch.py

@@ -0,0 +1,96 @@
+# -*- coding: utf-8 -*-
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import (
+    OpaqueFunction,
+    DeclareLaunchArgument,
+    TimerAction,
+    IncludeLaunchDescription,
+)
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from ament_index_python.packages import get_package_share_directory
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition, UnlessCondition
+
+
+def launch_setup(context, *args, **kwargs):
+    # Configure some helper variables and paths
+    pkg_c300_navigation = get_package_share_directory("c300_navigation")
+    pkg_driver = get_package_share_directory("c300_driver")
+    pkg_robot_description = get_package_share_directory("c300_description")
+    pkg_bringup = get_package_share_directory("c300_bringup")
+
+    # Hardware LIDAR
+    lidar_launch_py = PythonLaunchDescriptionSource(
+        [
+            pkg_bringup,
+            "/launch/urg_node_launch.py",
+        ]
+    )
+    lidar_launch = IncludeLaunchDescription(
+        lidar_launch_py,
+    )
+
+    # Hardware Driver
+    driver_launch_py = PythonLaunchDescriptionSource(
+        [
+            pkg_driver,
+            "/launch/driver.launch.py",
+        ]
+    )
+    driver_launch = IncludeLaunchDescription(
+        driver_launch_py,
+    )
+
+    # Parse xacro and publish robot state
+    robot_description_path = os.path.join(
+        pkg_robot_description, "urdf", "c300_base.urdf"
+    )
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            robot_description_path,
+            " ",
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="c300_robot_state_publisher",
+        output="both",
+        parameters=[robot_description, {"publish_frequency", "50"}],
+    )
+
+    # Bringup Navigation2
+    nav2_launch_py = PythonLaunchDescriptionSource(
+        [
+            pkg_c300_navigation,
+            "/launch/navigation.launch.py",
+        ]
+    )
+    nav2_launch = IncludeLaunchDescription(
+        nav2_launch_py,
+    )
+
+    nodes_and_launches = [
+        robot_state_publisher_node,
+        lidar_launch,
+        driver_launch,
+        TimerAction(period=5.0, actions=[nav2_launch]),
+    ]
+
+    return nodes_and_launches
+
+
+def generate_launch_description():
+    return LaunchDescription([OpaqueFunction(function=launch_setup)])

c300/c300_bringup/launch/urg_node_launch.py

@@ -0,0 +1,48 @@
+# -*- coding: utf-8 -*-
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import OpaqueFunction
+from launch.actions import SetLaunchConfiguration
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    config_dir = get_package_share_directory("c300_bringup")
+    launch_description = LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "sensor_interface",
+                default_value="ethernet",
+                description="sensor_interface: supported: serial, ethernet",
+            )
+        ]
+    )
+
+    def expand_param_file_name(context):
+        param_file = os.path.join(
+            config_dir,
+            "config",
+            "urg_node_" + context.launch_configurations["sensor_interface"] + ".yaml",
+        )
+        if os.path.exists(param_file):
+            return [SetLaunchConfiguration("param", param_file)]
+
+    param_file_path = OpaqueFunction(function=expand_param_file_name)
+    launch_description.add_action(param_file_path)
+
+    hokuyo_node = Node(
+        package="urg_node",
+        executable="urg_node_driver",
+        output="screen",
+        name="urg_node",
+        parameters=[LaunchConfiguration("param")],
+    )
+
+    launch_description.add_action(hokuyo_node)
+    return launch_description

c300/c300_driver/c300_driver/diff_drive_odom.py

@@ -37,10 +37,10 @@ def step(self, position, velocity):
         theta = self._radius * (wheel_r - wheel_l) / self._separation
         self._robot_pose = (
             self._robot_pose[0] + delta_s * cos(self._robot_pose[2] + (theta / 2.0)),
-            self._robot_pose[1] + delta_s * sin(self._robot_pose[2] + (theta / 2.0)),
+            self._robot_pose[1] - delta_s * sin(self._robot_pose[2] + (theta / 2.0)),
             self._robot_pose[2] + theta,
         )
-        q = quaternion_from_euler(0.0, 0.0, self._robot_pose[2])
+        q = quaternion_from_euler(0.0, 0.0, -self._robot_pose[2])
 
         self._last_time = now
 

c300/c300_driver/c300_driver/roboclaw_3.py

@@ -14,6 +14,12 @@ def __init__(self, comport, rate, timeout=0.01, retries=3):
         self.timeout = timeout
         self._trystimeout = retries
         self._crc = 0
+        self._port = serial.Serial(
+            port=self.comport,
+            baudrate=self.rate,
+            timeout=1,
+            interCharTimeout=self.timeout,
+        )
 
     # Command Enums
     class Cmd:
@@ -1195,15 +1201,3 @@ def WriteEeprom(self, address, ee_address, ee_word):
                 if tries == 0:
                     break
         return False
-
-    def Open(self):
-        try:
-            self._port = serial.Serial(
-                port=self.comport,
-                baudrate=self.rate,
-                timeout=1,
-                interCharTimeout=self.timeout,
-            )
-        except:
-            return 0
-        return 1

c300/c300_driver/c300_driver/twist2roboclaw.py

@@ -19,18 +19,52 @@
 from . import roboclaw_3
 from . import diff_drive_odom
 
+from tf2_ros import TransformBroadcaster
+
+from geometry_msgs.msg import TransformStamped
 from geometry_msgs.msg import Twist
+from sensor_msgs.msg import LaserScan, JointState
+from nav_msgs.msg import Odometry
+
+from std_msgs.msg import Header
 
 import math
 
 from pprint import pprint
 
 
+def euler_from_quaternion(odom_msg):
+    """
+    Convert a quaternion into euler angles (roll, pitch, yaw)
+    roll is rotation around x in radians (counterclockwise)
+    pitch is rotation around y in radians (counterclockwise)
+    yaw is rotation around z in radians (counterclockwise)
+    """
+    x = odom_msg.pose.pose.orientation.x
+    y = odom_msg.pose.pose.orientation.y
+    z = odom_msg.pose.pose.orientation.z
+    w = odom_msg.pose.pose.orientation.w
+    t0 = +2.0 * (w * x + y * z)
+    t1 = +1.0 - 2.0 * (x * x + y * y)
+    roll_x = math.atan2(t0, t1)
+
+    t2 = +2.0 * (w * y - z * x)
+    t2 = +1.0 if t2 > +1.0 else t2
+    t2 = -1.0 if t2 < -1.0 else t2
+    pitch_y = math.asin(t2)
+
+    t3 = +2.0 * (w * z + x * y)
+    t4 = +1.0 - 2.0 * (y * y + z * z)
+    yaw_z = math.atan2(t3, t4)
+
+    return roll_x, pitch_y, yaw_z  # in radians
+
+
 class RoboclawTwistSubscriber(Node):
     def __init__(self):
         super().__init__("roboclaw_twist_subscriber")
 
-        cmd_vel_topic = "cmd_vel"
+        cmd_vel_topic = "c3pzero/cmd_vel"
         self.wheel_radius = 0.1715  # meters
         self.wheel_circumference = 2 * math.pi * self.wheel_radius  # meters
         self.ppr = 11600  # pulses per wheel revolution
@@ -40,11 +74,14 @@ def __init__(self):
             Twist, cmd_vel_topic, self.twist_listener_callback, 10
         )
         self.subscription  # prevent unused variable warning
-
-        self.rc = roboclaw_3.Roboclaw("/dev/ttyACM0", 115200)
-
-        if not self.rc.Open():
+        self.odom_publisher_ = self.create_publisher(Odometry, "c3pzero/odometry", 10)
+        self.br = TransformBroadcaster(self)
+        self.joint_publisher_ = self.create_publisher(JointState, "joint_states", 10)
+        try:
+            self.rc = roboclaw_3.Roboclaw("/dev/ttyACM0", 115200)
+        except:
             self.get_logger().error("failed to open port")
+            raise
         self.rc_address = 0x80
 
         version = self.rc.ReadVersion(self.rc_address)
@@ -68,9 +105,11 @@ def twist_listener_callback(self, msg):
         # self.get_logger().info('X_vel: %f, Z_rot: %f' % (0.4*msg.linear.x, msg.angular.z))
 
         right_wheel = (
-            0.2 * msg.linear.x + (0.3 * msg.angular.z * self.wheel_track) / 2
+            msg.linear.x + (msg.angular.z * self.wheel_track) / 2
+        )  # meters / sec
+        left_wheel = (
+            msg.linear.x - (msg.angular.z * self.wheel_track) / 2
         )  # meters / sec
-        left_wheel = 0.2 * msg.linear.x - (0.3 * msg.angular.z * self.wheel_track) / 2
 
         wheel_cmds = self.mps_to_pps((right_wheel, left_wheel))
         self.rc.SpeedM1(self.rc_address, wheel_cmds[0])
@@ -106,14 +145,38 @@ def odom_callback(self):
         odom_msg = self.diff_drive_odom.step(wheel_pos, wheel_speed)
         # pprint(odom_msg.pose.pose.position)
 
-        self.get_logger().info(
+        self.get_logger().debug(
             "Pose: x=%f, y=%f theta=%f"
             % (
                 odom_msg.pose.pose.position.x,
                 odom_msg.pose.pose.position.y,
-                odom_msg.pose.pose.orientation.z,
+                euler_from_quaternion(odom_msg)[2],
             )
         )
+        self.odom_publisher_.publish(odom_msg)
+
+        t = TransformStamped()
+        t.header.stamp = self.get_clock().now().to_msg()
+        t.header.frame_id = "odom"
+        t.child_frame_id = "base_link"
+        t.transform.translation.x = odom_msg.pose.pose.position.x
+        t.transform.translation.y = odom_msg.pose.pose.position.y
+        t.transform.translation.z = 0.0
+        t.transform.rotation.x = odom_msg.pose.pose.orientation.x
+        t.transform.rotation.y = odom_msg.pose.pose.orientation.y
+        t.transform.rotation.z = odom_msg.pose.pose.orientation.z
+        t.transform.rotation.w = odom_msg.pose.pose.orientation.w
+
+        # Send the transformation
+        self.br.sendTransform(t)
+
+        wheel_state = JointState()
+        wheel_state.header.stamp = self.get_clock().now().to_msg()
+        wheel_state.name = ["drivewhl_r_joint", "drivewhl_l_joint"]
+        wheel_state.position = wheel_pos
+        wheel_state.velocity = wheel_speed
+        wheel_state.effort = []
+        self.joint_publisher_.publish(wheel_state)
 
     def mps_to_pps(self, wheel_speed):
         right_wheel_pluses = int(wheel_speed[0] / self.wheel_circumference * self.ppr)

c300/c300_driver/launch/driver.launch.py

@@ -0,0 +1,23 @@
+# -*- coding: utf-8 -*-
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+import launch
+import launch_ros.actions
+
+
+def generate_launch_description():
+    return launch.LaunchDescription(
+        [
+            launch_ros.actions.Node(
+                package="c300_driver",
+                executable="twist2roboclaw",
+                name="driver",
+                remappings={
+                    ("c3pzero/cmd_vel", "/cmd_vel"),
+                    ("/c3pzero/odometry", "/odom"),
+                },
+            ),
+        ]
+    )

c300/c300_navigation/launch/navigation.launch.py

@@ -64,7 +64,7 @@ def generate_launch_description():
     )
 
     declare_slam_cmd = DeclareLaunchArgument(
-        "slam", default_value="False", description="Whether run a SLAM"
+        "slam", default_value="True", description="Whether run a SLAM"
     )
 
     declare_map_yaml_cmd = DeclareLaunchArgument(
@@ -75,7 +75,7 @@ def generate_launch_description():
 
     declare_use_sim_time_cmd = DeclareLaunchArgument(
         "use_sim_time",
-        default_value="True",
+        default_value="False",
         description="Use simulation (Gazebo) clock if True",
     )