CODEWITHSUNDEEP
navigationrosslams
Dima Vashchenko
Dima Vashchenko

Reputation: 1

Setup Nav2 with ROS jazzy on real hardware

I want to build mobile robot. I am using raspberry pi 5, YDLidar X3, ROS jazzy, slam toolbox and want to add navigation to my robot(nav2). Also I should mention that I am using motors without encoders.

Is it possible to run nav2 without encoders? Currently I am using odometry from lidar using this package: https://github.com/MAPIRlab/rf2o_laser_odometry

My transforms looks like this

  1. my xacro file looks like
<robot name="my_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <!-- Constants/Properties -->
    <xacro:property name="base_width" value="0.4"/>
    <xacro:property name="base_length" value="0.6"/>
    <xacro:property name="base_height" value="0.2"/>
    
    <xacro:property name="wheel_radius" value="0.1"/>
    <xacro:property name="wheel_length" value="0.05"/>
    <xacro:property name="wheel_offset_z" value="-0.1"/>
    <xacro:property name="wheel_offset_y" value="0.2"/>
    <xacro:property name="wheel_offset_x" value="-0.1"/>
    
    <xacro:property name="caster_radius" value="0.05"/>
    <xacro:property name="caster_offset_x" value="0.2"/>
    <xacro:property name="caster_offset_z" value="-0.15"/>
    
    <xacro:property name="lidar_radius" value="0.1"/>
    <xacro:property name="lidar_length" value="0.06"/>
    <xacro:property name="lidar_offset_z" value="0.13"/>

    <!-- Materials -->
    <material name="grey">
        <color rgba="0.5 0.5 0.5 1"/>
    </material>
    <material name="blue">
        <color rgba="0 0 0.5 1"/>
    </material>

    <!-- Wheel macro -->
    <xacro:macro name="wheel" params="prefix reflect">
        <joint name="base_${prefix}_wheel_joint" type="fixed">
            <parent link="base_link"/>
            <child link="${prefix}_wheel"/>
            <origin xyz="${wheel_offset_x} ${reflect * wheel_offset_y} ${wheel_offset_z}" rpy="0 0 0"/>
        </joint>
        <link name="${prefix}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
                </geometry>
                <origin xyz="0 0 0" rpy="1.57 0 0"/>
                <material name="grey"/>
            </visual>
        </link>
    </xacro:macro>

    <!-- Base Footprint -->
    <link name="base_footprint"/>
    
    <joint name="base_joint" type="fixed">
        <parent link="base_footprint"/>
        <child link="base_link"/>
        <origin xyz="0 0 ${base_height/2}" rpy="0 0 0"/>
    </joint>

    <!-- Base Link -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="${base_length} ${base_width} ${base_height}"/>
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <material name="blue"/>
        </visual>
    </link>

    <!-- LIDAR -->
    <joint name="lidar_joint" type="fixed">
        <parent link="base_link"/>
        <child link="lidar_link"/>
        <origin xyz="0 0 ${lidar_offset_z}" rpy="0 0 0"/>
    </joint>
    
    <link name="lidar_link">
        <visual>
            <geometry>
                <cylinder radius="${lidar_radius}" length="${lidar_length}"/>
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <material name="grey"/>
        </visual>
    </link>

    <!-- Wheels -->
    <xacro:wheel prefix="left" reflect="1"/>
    <xacro:wheel prefix="right" reflect="-1"/>

    <!-- Caster Wheel -->
    <joint name="base_caster_wheel_joint" type="fixed">
        <parent link="base_link"/>
        <child link="caster_wheel"/>
        <origin xyz="${caster_offset_x} 0 ${caster_offset_z}" rpy="0 0 0"/>
    </joint>
    
    <link name="caster_wheel">
        <visual>
            <geometry>
                <sphere radius="${caster_radius}"/>
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <material name="grey"/>
        </visual>
    </link>
</robot>
  1. I have move listener node for moving robot
import rclpy from rclpy.node import Node import gpiod from geometry_msgs.msg import Twist import time

class ArduinoBaseController(Node): def init(self): super().init('arduino_base_controller')

    # Define GPIO pins
    self.motor_left_forward = 17   # Left motor forward
    self.motor_left_backward = 27  # Left motor backward
    self.motor_right_forward = 22  # Right motor forward
    self.motor_right_backward = 23 # Right motor backward
    
    self.chip = gpiod.Chip('gpiochip4')

    # Initialize GPIOs
    self.left_forward = self.chip.get_line(self.motor_left_forward)
    self.left_backward = self.chip.get_line(self.motor_left_backward)
    self.right_forward = self.chip.get_line(self.motor_right_forward)
    self.right_backward = self.chip.get_line(self.motor_right_backward)
    
    self.left_forward.request(consumer="Motor", type=gpiod.LINE_REQ_DIR_OUT)
    self.left_backward.request(consumer="Motor", type=gpiod.LINE_REQ_DIR_OUT)
    self.right_forward.request(consumer="Motor", type=gpiod.LINE_REQ_DIR_OUT)
    self.right_backward.request(consumer="Motor", type=gpiod.LINE_REQ_DIR_OUT)

    # Subscribe to /cmd_vel
    self.create_subscription(Twist, '/cmd_vel', self.cmd_vel_callback, 10)
    self.get_logger().info("Arduino Base Controller Node Initialized. Listening on /cmd_vel.")

def stop_motors(self):
    """ Stops all motors safely """
    self.left_forward.set_value(0)
    self.left_backward.set_value(0)
    self.right_forward.set_value(0)
    self.right_backward.set_value(0)

def cmd_vel_callback(self, msg: Twist):
    linear = msg.linear.x
    angular = msg.angular.z
    self.get_logger().info(f"Received cmd_vel: linear.x = {linear}, angular.z = {angular}")
    
    if linear > 0.05:  # Move Forward
        self.get_logger().info("Command: Move Forward")
        self.left_forward.set_value(0)
        self.left_backward.set_value(1)
        self.right_forward.set_value(1)
        self.right_backward.set_value(0)

    elif linear < -0.05:  # Move Backward
        self.get_logger().info("Command: Move Backward")
        self.left_forward.set_value(1)
        self.left_backward.set_value(0)
        self.right_forward.set_value(0)
        self.right_backward.set_value(1)

    elif angular > 0.05:  # Turn Left
        self.get_logger().info("Command: Turn Left")
        self.left_forward.set_value(0)
        self.left_backward.set_value(1)
        self.right_forward.set_value(0)
        self.right_backward.set_value(1)

    elif angular < -0.05:  # Turn Right
        self.get_logger().info("Command: Turn Right")
        self.left_forward.set_value(1)
        self.left_backward.set_value(0)
        self.right_forward.set_value(1)
        self.right_backward.set_value(0)

    else:  # Stop
        self.get_logger().info("Command: Stop")
        self.stop_motors()
        

def destroy_node(self):
    """ Release GPIOs on shutdown """
    self.stop_motors()
    self.left_forward.release()
    self.left_backward.release()
    self.right_forward.release()
    self.right_backward.release()
    super().destroy_node()

def main(args=None): rclpy.init(args=args) node = ArduinoBaseController() rclpy.spin(node) node.destroy_node() rclpy.shutdown()

if name == 'main': main()

I start it as

ros2 run tf2_ros static_transform_publisher 0 0 0.13 0 0 0 base_link lidar_link
ros2 run move_listener move_listener   (topic is published and listen to data correctly)
ros2 run robot_state_publisher robot_state_publisher /home/dima/ros2_ws/src/my_robot.urdf
ros2 launch ydlidar_ros2_driver ydlidar_launch_view.py
ros2 launch rf2o_laser_odometry rf2o_laser_odometry.launch.py
ros2 launch nav2_bringup localization_launch.py map:=/home/dima/ros2_ws/src/maps/room.yaml (it works correctly and I can locate robot)

Then I run navigation:

ros2 launch nav2_bringup navigation_launch.py use_sim_time:=False

and when I setup Goal Pose in logs I receive

[bt_navigator-5] [INFO] [1740857782.106966815] [bt_navigator]: Begin navigating from current location (0.26, -0.28) to (1.32, -0.26)
[controller_server-1] [INFO] [1740857782.141517124] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [WARN] [1740857782.141642051] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded general_goal_checker . This warning will appear once.
[controller_server-1] [WARN] [1740857782.141674366] [controller_server]: No progress checker was specified in parameter 'current_progress_checker'. Server will use only plugin loaded progress_checker . This warning will appear once.
[controller_server-1] [WARN] [1740857782.196578189] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 18.2448 Hz.
[bt_navigator-5] [WARN] [1740857782.197324103] [bt_navigator_navigate_to_pose_rclcpp_node]: Timed out while waiting for action server to acknowledge goal request for follow_path
[bt_navigator-5] [WARN] [1740857782.197842478] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[bt_navigator-5] [ERROR] [1740857782.197984424] [bt_navigator]: Goal failed
[controller_server-1] [WARN] [1740857782.269227152] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 13.7792 Hz.
[controller_server-1] [WARN] [1740857782.313207748] [controller_server]: Control loop missed its desired rate of 20.0000 Hz. Current loop rate is 22.8437 Hz.
[collision_monitor-8] [ERROR] [1740857782.539174449] [getTransform]: Failed to get "laser_frame"->"base_footprint" frame transform: Lookup would require extrapolation into the future.  Requested time 1740857782.333532 but the latest data is at time 1740857782.161137, when looking up transform from frame [odom] to frame [base_footprint]
[collision_monitor-8] [WARN] [1740857782.539311117] [collision_monitor]: Robot to stop due to invalid source. Either due to data not published yet, or to lack of new data received within the sensor timeout, or if impossible to transform data to base frame

Could you please help me to resolve it? I would really appreciate any piece of advise, thank you

Upvotes: 0

Views: 9

Answers (0)

Related Questions