search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.59k stars 1.3k forks source link

Errors with controller_server using GPS for Navigation #4359

Closed arad2456 closed 5 months ago

arad2456 commented 5 months ago

Problem Description

I am working with ROS2 Humble and trying to implement a navigation algorithm that integrates GPS measurements. I successfully ran a simulation in Gazebo using the nav2_gps_waypoint_follower_demo with the turtlebot, after adjusting some configurations. When I tried to adapt this approach for my custom robot using a launch file, I encountered the following errors during the nav2_bringup process:

[controller_server-10] [ERROR] [1716328246.091536224] [controller_server]: Couldn't load critics! Caught exception: No critics defined for FollowPath
[controller_server-10] [ERROR] [1716328246.096419625] [controller_server]: Caught exception in callback for transition 10
[controller_server-10] [ERROR] [1716328246.096489476] [controller_server]: Original error: No critics defined for FollowPath
[controller_server-10] [WARN] [1716328246.096507160] [controller_server]: Error occurred while doing error handling.
[controller_server-10] [FATAL] [1716328246.096518289] [controller_server]: Lifecycle node controller_server does not have error state implemented
[lifecycle_manager-17] [ERROR] [1716328246.097113046] [lifecycle_manager_navigation]: Failed to change state for node: controller_server
[lifecycle_manager-17] [ERROR] [1716328246.097165270] [lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.

As indicated, the nav2_bringup process fails to launch, and thus, the navigation fails. I would appreciate any assistance you can provide.

Package Download nav2_gps_waypoint_follower_demo.zip

For reproducibility, I have uploaded the package containing all necessary configuration files, including the problematic launch file, _run_nav2gps.launch.py, and the launch file from the documentation that worked, _gps_waypointfollower.launch.py. In the file _gps_waypointfollower.launch.py you have to first change the topic of the map in rviz to a different topic, and only then the navigation works.

Launch and Urdf Files

This is the launch file _gps_waypointfollower.launch.py:

# Copyright (c) 2018 Intel Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, Command
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
from nav2_common.launch import RewrittenYaml
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # Get the launch directory
    bringup_dir = get_package_share_directory('nav2_bringup')
    gps_wpf_dir = get_package_share_directory("nav2_gps_waypoint_follower_demo")
    launch_dir = os.path.join(gps_wpf_dir, 'launch')
    params_dir = os.path.join(gps_wpf_dir, "config")
    nav2_params = os.path.join(params_dir, "nav2_no_map_params.yaml")
    configured_params = RewrittenYaml(
        source_file=nav2_params, root_key="", param_rewrites="", convert_types=True
    )

    pkg_share = get_package_share_directory('nav2_gps_waypoint_follower_demo')
    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
    path_lafette = os.path.join(pkg_share, 'urdf', 'Lafette_small_urdf.urdf')

    use_rviz = LaunchConfiguration('use_rviz')

    declare_use_rviz_cmd = DeclareLaunchArgument(
        'use_rviz',
        default_value='True',
        description='Whether to start RVIZ')

    launch_gazebo_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(pkg_share, 'launch', 'launch_lafette_gazebo.launch.py'))
    )

    robot_localization_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_dir, 'dual_ekf_navsat_new.launch.py'))
    )

    navigation2_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(bringup_dir, "launch", "navigation_launch.py")),
        launch_arguments={
            "use_sim_time": "True",
            "params_file": configured_params,
            "autostart": "True",
        }.items(),
    )

    rviz_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(bringup_dir, "launch", 'rviz_launch.py')),
        condition=IfCondition(use_rviz)
    )

    # Create the launch description and populate
    ld = LaunchDescription()

    # rviz launch
    ld.add_action(declare_use_rviz_cmd)
    ld.add_action(rviz_cmd)

    #interfaces
    ld.add_action(launch_gazebo_cmd)

    # robot localization launch
    ld.add_action(robot_localization_cmd)

    # navigation2 launch
    ld.add_action(navigation2_cmd)

    return ld

This is my robot's urdf file _lafette_smallurdf.urdf:

<?xml version="1.0"?>
<robot name="Lafette" xmlns:xacro="http://ros.org/wiki/xacro">

  <!-- All units are in SI -->

  <!-- Define robot constants -->
  <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="lafette_mass" value="5"/>
  <xacro:property name="wheel_mass" value="0.5"/>

  <xacro:property name="wheel_radius" value="0.1025"/>
  <xacro:property name="wheel_width" value="0.075"/>
  <xacro:property name="wheel_zoff" value="${base_height / 2}"/>
  <xacro:property name="wheel_yoff" value="${base_width / 2}"/>
  <xacro:property name="wheel_xoff" value="${base_length / 2}"/>

<!-- Define intertial property macros  -->
  <xacro:macro name="box_inertia" params="m w h d">
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="cylinder_inertia" params="m r h">
    <inertial>
      <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
      <mass value="${m}"/>
      <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
    </inertial>
  </xacro:macro>

  <xacro:macro name="sphere_inertia" params="m r">
    <inertial>
      <mass value="${m}"/>
      <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
    </inertial>
  </xacro:macro>

  <!-- Robot Base -->
  <link name="base_link">
    <visual>
      <origin xyz="0.0 0.0 ${(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>

    <collision>
      <origin xyz="0.0 0.0 ${(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
    </collision>

    <xacro:box_inertia m="${lafette_mass}" w="${base_width}" d="${base_length}" h="${base_height}"/>

  </link>

    <!-- Robot Footprint (non physical link for ROS, we need to bring the CoG of the robot to the cntral plane on the ground) -->
  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_footprint"/>
    <origin xyz="0.0 0.0 ${(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
  </joint>

<link name="imu_link">
  <visual>
    <geometry>
      <box size="0.1 0.1 0.1"/>
    </geometry>
  </visual>

  <collision>
    <geometry>
      <box size="0.1 0.1 0.1"/>
    </geometry>
  </collision>

  <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
</link>

<joint name="imu_joint" type="fixed">
  <parent link="base_link"/>
  <child link="imu_link"/>
  <origin xyz="0 -0.1 ${wheel_radius + base_height + 0.1/2}"/>
</joint>

 <gazebo reference="imu_link">
  <sensor name="imu_sensor" type="imu">
   <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
      <ros>
        <remapping>~/out:=imu</remapping>
      </ros>
      <initial_orientation_as_reference>false</initial_orientation_as_reference>
    </plugin>
    <always_on>true</always_on>
    <update_rate>100</update_rate>
    <visualize>true</visualize>
    <imu>
      <angular_velocity>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </noise>
        </z>
      </angular_velocity>
      <linear_acceleration>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </noise>
        </z>
      </linear_acceleration>
    </imu>
  </sensor>
</gazebo>

<!-- Wheels -->
  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">
    <link name="${prefix}_link">
      <visual>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <material name="Gray">
          <color rgba="0.5 0.5 0.5 1.0"/>
        </material>
      </visual>

      <collision>
        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
      </collision>

      <xacro:cylinder_inertia m="${wheel_mass}" r="${wheel_radius}" h="${wheel_width}"/>

    </link>

    <joint name="${prefix}_joint" type="continuous">
      <parent link="base_link"/>
      <child link="${prefix}_link"/>
      <origin xyz="${x_reflect*(wheel_xoff - 2*wheel_radius)} ${y_reflect*(wheel_yoff + wheel_width/2)} ${wheel_radius}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>

<!-- indices: f-front, b-back, l-left, r-right -->
  <xacro:wheel prefix="drivewhl_fl" x_reflect="2" y_reflect="1" />
  <xacro:wheel prefix="drivewhl_fr" x_reflect="2" y_reflect="-1" />
  <xacro:wheel prefix="drivewhl_bl" x_reflect="-2" y_reflect="1" />
  <xacro:wheel prefix="drivewhl_br" x_reflect="-2" y_reflect="-1" />

<gazebo>

 <!-- <plugin name='skid_steer_drive' filename='libgazebo_ros_diff_drive.so'> -->
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>

        <!-- Number of wheel pairs -->
        <num_wheel_pairs>2</num_wheel_pairs>

        <!-- wheels0 -->
        <left_joint>drivewhl_fl_joint</left_joint>
        <right_joint>drivewhl_fr_joint</right_joint>

        <!-- wheels1-->
        <left_joint>drivewhl_bl_joint</left_joint>
        <right_joint>drivewhl_br_joint</right_joint>

        <!-- kinematics -->
        <wheel_separation>0.6</wheel_separation>
        <wheel_diameter>0.2</wheel_diameter>

        <!-- limits -->
        <max_wheel_torque>20</max_wheel_torque>
        <max_wheel_acceleration>1.0</max_wheel_acceleration>

        <!-- output -->
        <publish_odom>true</publish_odom>
        <publish_odom_tf>true</publish_odom_tf>
        <publish_wheel_tf>true</publish_wheel_tf>

        <odometry_frame>odom</odometry_frame>
        <robot_base_frame>base_link</robot_base_frame>

      </plugin>

</gazebo>

<link name="camera_link">
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <box size="0.015 0.130 0.022"/>
    </geometry>
  </visual>

  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <box size="0.015 0.130 0.022"/>
    </geometry>
  </collision>

  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <mass value="0.035"/>
    <inertia ixx="0.001"  ixy="0"  ixz="0" iyy="0.001" iyz="0" izz="0.001" />
  </inertial>
</link>

<joint name="camera_joint" type="fixed">
  <parent link="base_link"/>
  <child link="camera_link"/>
  <origin xyz="${base_length/2} 0 ${wheel_radius + base_height/8}" rpy="0 0.78 0"/>
</joint>

<link name="camera_depth_frame"/>

<joint name="camera_depth_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
  <parent link="camera_link"/>
  <child link="camera_depth_frame"/>
</joint>

<gazebo reference="camera_link">
  <sensor name="depth_camera" type="depth">
    <visualize>true</visualize>
    <update_rate>30.0</update_rate>
    <camera name="camera">
      <horizontal_fov>1.047198</horizontal_fov>
      <image>
        <width>640</width>
        <height>480</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.05</near>
        <far>3</far>
      </clip>
    </camera>
    <plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
      <baseline>0.2</baseline>
      <alwaysOn>true</alwaysOn>
      <updateRate>0.0</updateRate>
      <frame_name>camera_depth_frame</frame_name>
      <pointCloudCutoff>0.5</pointCloudCutoff>
      <pointCloudCutoffMax>3.0</pointCloudCutoffMax>
      <distortionK1>0</distortionK1>
      <distortionK2>0</distortionK2>
      <distortionK3>0</distortionK3>
      <distortionT1>0</distortionT1>
      <distortionT2>0</distortionT2>
      <CxPrime>0</CxPrime>
      <Cx>0</Cx>
      <Cy>0</Cy>
      <focalLength>0</focalLength>
      <hackBaseline>0</hackBaseline>
    </plugin>
  </sensor>
</gazebo>

<link name="ultrasonic_link">
  <visual>
    <origin xyz="0 0 ${0.055/2}" rpy="0 0 0"/>
    <geometry>
      <box size="0.015 0.130 0.022"/>
    </geometry>
  </visual>

  <collision>
    <origin xyz="0 0 ${0.055/2}" rpy="0 0 0"/>
    <geometry>
      <box size="0.015 0.130 0.022"/>
    </geometry>
  </collision>

  <inertial>
    <origin xyz="0 0 ${0.055/2}" rpy="0 0 0"/>
    <mass value="0.035"/>
    <inertia ixx="0.001"  ixy="0"  ixz="0" iyy="0.001" iyz="0" izz="0.001" />
  </inertial>
</link>

<joint name="ultrasonic_joint" type="fixed">
  <parent link="base_link"/>
  <child link="ultrasonic_link"/>
  <origin xyz="${base_length/2} 0 ${wheel_radius + base_height/2}" rpy="0 0 0"/>
</joint>

<gazebo reference="ultrasonic_link">
  <sensor name="ultrasonic_sensor" type="ray">
    <always_on>true</always_on>
    <visualize>false</visualize>
    <update_rate>5</update_rate>
    <ray>
      <scan>
        <horizontal>
          <samples>5</samples>
          <resolution>1.000000</resolution>
          <min_angle>-0.12</min_angle>
          <max_angle>0.12</max_angle>
        </horizontal>
        <vertical>
          <samples>5</samples>
          <resolution>1.000000</resolution>
          <min_angle>-0.01</min_angle>
          <max_angle>0.01</max_angle>
        </vertical>
      </scan>
      <range>
        <min>0.02</min>
        <max>4</max>
        <resolution>0.01</resolution>
      </range>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.01</stddev>
      </noise>
    </ray>
    <plugin name="ultrasonic_sensor_1" filename="libgazebo_ros_ray_sensor.so">
      <ros>
        <remapping>~/out:=ultrasonic_sensor</remapping>
      </ros>
      <output_type>sensor_msgs/Range</output_type>
      <radiation_type>ultrasound</radiation_type>
      <frame_name>ultrasonic_link</frame_name>
    </plugin>
  </sensor>  
</gazebo>

<link name="GNSS_link">
  <visual>
    <geometry>
      <box size="0.1 0.1 0.1"/>
    </geometry>
  </visual>

  <collision>
    <geometry>
      <box size="0.1 0.1 0.1"/>
    </geometry>
  </collision>

  <xacro:box_inertia m="0.001" w="0.001" d="0.001" h="0.001"/>
</link>

<joint name="GNSS_joint" type="fixed">
  <parent link="base_link"/>
  <child link="GNSS_link"/>
  <origin xyz="0 0.05 ${wheel_radius + base_height + 0.1/2}"/>
</joint>

<gazebo reference="GNSS_link">
<sensor name="tb3_gps" type="gps">
  <always_on>true</always_on>
  <update_rate>1</update_rate>
  <pose>0 0 0 0 0 0</pose>
  <gps>
    <position_sensing>
      <horizontal>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </horizontal>
      <vertical>
        <noise type="gaussian">
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </vertical>
    </position_sensing>
  </gps>
  <plugin name="my_gps_plugin" filename="libgazebo_ros_gps_sensor.so">
    <ros>
      <remapping>~/out:=gps/fix</remapping>
    </ros>
  </plugin>
</sensor>
</gazebo>

 <link name="lidar_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
         <cylinder radius="0.0508" length="0.055"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
         <cylinder radius="0.0508" length="0.055"/>
      </geometry>
    </visual>

    <xacro:cylinder_inertia m="0.01" r="0.0508" h="0.055"/>
  </link>

  <joint name="lidar_joint" type="fixed">
    <parent link="base_link"/>
    <child link="lidar_link"/>
      <origin xyz="0.2 0 ${wheel_radius + base_height + 0.1/2 + 0.2}"/>
  </joint>

  <gazebo reference="lidar_link">
    <sensor name="lidar" type="ray">
      <always_on>true</always_on>
      <visualize>true</visualize>
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1.000000</resolution>
            <min_angle>0.000000</min_angle>
            <max_angle>6.280000</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.120000</min>
          <max>3.5</max>
          <resolution>0.015000</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="scan" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <remapping>~/out:=scan</remapping>
        </ros>
        <output_type>sensor_msgs/LaserScan</output_type>
        <frame_name>lidar_link</frame_name>
      </plugin>
    </sensor>
  </gazebo>

</robot>
SteveMacenski commented 5 months ago

Please ask on robotics stack exchange but your error seems clear to me from your logs'

[controller_server-10] [ERROR] [1716328246.096489476] [controller_server]: Original error: No critics defined for FollowPath

You need to pass in parameters. This is likely an issue with your launch file.

arad2456 commented 5 months ago

Please ask on robotics stack exchange but your error seems clear to me from your logs'

[controller_server-10] [ERROR] [1716328246.096489476] [controller_server]: Original error: No critics defined for FollowPath

You need to pass in parameters. This is likely an issue with your launch file.

I tried on different occasions to ask three different questions on Robotics Stack Exchange, but from my experience you rarely get an answer there if you ask a question which is not completly basic. I know this forum is for problems with the package, and I understand that you don't have or need to help me with the problem in my code. Yet, I would be extremely thankful if you could help me here. I tried to pass in parameters which also contain the critiques. This is the calling for nav2_bringup from the launch file:

    navigation2_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(bringup_dir, "launch", "navigation_launch.py")),
        launch_arguments={
            "use_sim_time": "True",
            "params_file": configured_params,
            "autostart": "True",
        }.items(),
    )

And this is the part with the critics in the configuration file when calling the controller_server:

...
controller_server:
  ros__parameters:
   ...
    FollowPath:
  ...
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
SteveMacenski commented 5 months ago

Unfortunately this is not an alternative for Q&A, that's what robotics stack exchange (or ROS Answers back when) is for. This is an issue tracker for project bugs and issues, not to help for first time setup. We as maintainers do not have the availability to help with every user's unique problems in getting up and running the first time (which often just has us pointing back to our own nav2_bringup files or other projects using Nav2 as successful examples).

Please see the Nav2 documentation https://docs.nav2.org/, successful working examples in this package, and other projects using and deploying Nav2 for help with your first-time setup.

arad2456 commented 5 months ago

Ok, I understand, thank you anyway :)