Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

nav2_gps_waypoint_follower_demo does not work on ros2 humble???! #77

Open
ZaBy10 opened this issue Dec 5, 2023 · 36 comments
Open

nav2_gps_waypoint_follower_demo does not work on ros2 humble???! #77

ZaBy10 opened this issue Dec 5, 2023 · 36 comments

Comments

@ZaBy10
Copy link

ZaBy10 commented Dec 5, 2023

Could someone please tell what modifications must be made in gps waypoint follower package for it to be used in ros2 humble...
Please provide us the solution,it would be a great help!!!!!

@LuukBerkel
Copy link

LuukBerkel commented Dec 14, 2023

The newest version of nav2, contains the function followGpsWaypoints(wps) service to follow Geoposes, but the version of nav2 for humble and foxy don't have this service, unfortunatly. They can only follow PoseStamped with the followWaypoints(wpl) service. Because this tutorial uses the followGpsWaypoints(wps) service, it only works on Iron> from what i have heard. The code below shows part of what is missing in the humble branch of nav2, but what is available in the main branch. Besides this big issue, there is a small issue of plugins changing per nav2 distro version. But this is easy to solve by changing the plugin names in the no_map_config_nav2.yaml file.

std::vector<geometry_msgs::msg::PoseStamped>
WaypointFollower::convertGPSPosesToMapPoses(
  const std::vector<geographic_msgs::msg::GeoPose> & gps_poses)
{
  RCLCPP_INFO(
    this->get_logger(), "Converting GPS waypoints to %s Frame..",
    global_frame_id_.c_str());

  std::vector<geometry_msgs::msg::PoseStamped> poses_in_map_frame_vector;
  int waypoint_index = 0;
  for (auto && curr_geopose : gps_poses) {
    auto request = std::make_shared<robot_localization::srv::FromLL::Request>();
    auto response = std::make_shared<robot_localization::srv::FromLL::Response>();
    request->ll_point.latitude = curr_geopose.position.latitude;
    request->ll_point.longitude = curr_geopose.position.longitude;
    request->ll_point.altitude = curr_geopose.position.altitude;

    from_ll_to_map_client_->wait_for_service((std::chrono::seconds(1)));
    if (!from_ll_to_map_client_->invoke(request, response)) {
      RCLCPP_ERROR(
        this->get_logger(),
        "fromLL service of robot_localization could not convert %i th GPS waypoint to"
        "%s frame, going to skip this point!"
        "Make sure you have run navsat_transform_node of robot_localization",
        waypoint_index, global_frame_id_.c_str());
      if (stop_on_failure_) {
        RCLCPP_ERROR(
          this->get_logger(),
          "Conversion of %i th GPS waypoint to"
          "%s frame failed and stop_on_failure is set to true"
          "Not going to execute any of waypoints, exiting with failure!",
          waypoint_index, global_frame_id_.c_str());
        return std::vector<geometry_msgs::msg::PoseStamped>();
      }
      continue;
    } else {
      geometry_msgs::msg::PoseStamped curr_pose_map_frame;
      curr_pose_map_frame.header.frame_id = global_frame_id_;
      curr_pose_map_frame.header.stamp = this->now();
      curr_pose_map_frame.pose.position = response->map_point;
      curr_pose_map_frame.pose.orientation = curr_geopose.orientation;
      poses_in_map_frame_vector.push_back(curr_pose_map_frame);
    }
    waypoint_index++;
  }
  RCLCPP_INFO(
    this->get_logger(),
    "Converted all %i GPS waypoint to %s frame",
    static_cast<int>(poses_in_map_frame_vector.size()), global_frame_id_.c_str());
  return poses_in_map_frame_vector;
}

The solution is as follows

Do the conversion yourself from Geopose to PoseStamped using robot_localization and then call the followWaypoints(wpl) service. I am in the process of rewriting the tutorial files to fix this. I can already share the rewritten version of the logged_waypoint_follower.py for humble, as that is already working for humble.

import rclpy
from nav2_simple_commander.robot_navigator import BasicNavigator
import yaml
from ament_index_python.packages import get_package_share_directory
import os
import sys
import time
from robot_localization.srv import FromLL
from rclpy.node import Node
from nav2_gps_waypoint_follower_demo.utils.gps_utils import latLonYaw2Geopose
from nav2_msgs.action import FollowWaypoints
from geometry_msgs.msg import PoseStamped


class YamlWaypointParser:
    """
    Parse a set of gps waypoints from a yaml file
    """

    def __init__(self, wps_file_path: str) -> None:
        with open(wps_file_path, 'r') as wps_file:
            self.wps_dict = yaml.safe_load(wps_file)

    def get_wps(self):
        """
        Get an array of geographic_msgs/msg/GeoPose objects from the yaml file
        """
        gepose_wps = []
        for wp in self.wps_dict["waypoints"]:
            latitude, longitude, yaw = wp["latitude"], wp["longitude"], wp["yaw"]
            gepose_wps.append(latLonYaw2Geopose(latitude, longitude, yaw))
        return gepose_wps


class GpsWpCommander(Node):
    """
    Class to use nav2 gps waypoint follower to follow a set of waypoints logged in a yaml file
    """

    def __init__(self, wps_file_path):
        super().__init__('minimal_client_async')
        self.navigator = BasicNavigator("basic_navigator")
        self.wp_parser = YamlWaypointParser(wps_file_path)
        self.localizer = self.create_client(FromLL,  '/fromLL')
        while not self.localizer.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')

    def start_wpf(self):
        """
        Function to start the waypoint following
        """
        self.navigator.waitUntilNav2Active(localizer='controller_server')
        wps = self.wp_parser.get_wps()

        wpl = []
        for wp in wps:
            self.req = FromLL.Request()
            self.req.ll_point.longitude = wp.position.longitude
            self.req.ll_point.latitude = wp.position.latitude
            self.req.ll_point.altitude = wp.position.altitude

            log = 'long{:f}, lat={:f}, alt={:f}'.format(self.req.ll_point.longitude, self.req.ll_point.latitude, self.req.ll_point.altitude)
            self.get_logger().info(log)

            self.future = self.localizer.call_async(self.req)
            rclpy.spin_until_future_complete(self, self.future)

            self.resp = PoseStamped()
            self.resp.header.frame_id = 'map'
            self.resp.header.stamp = self.get_clock().now().to_msg()
            self.resp.pose.position = self.future.result().map_point

            log = 'x={:f}, y={:f}, z={:f}'.format(self.future.result().map_point.x, self.future.result().map_point.y, self.future.result().map_point.z)
            self.get_logger().info(log)
            
            self.resp.pose.orientation = wp.orientation
            wpl += [self.resp]

        self.navigator.followWaypoints(wpl)
        print("wps completed successfully")

def main():
    rclpy.init()

    # allow to pass the waypoints file as an argument
    default_yaml_file_path = os.path.join(get_package_share_directory(
        "node"), "config", "demo_waypoints.yaml")
    if len(sys.argv) > 1:
        yaml_file_path = sys.argv[1]
    else:
        yaml_file_path = default_yaml_file_path

    gps_wpf = GpsWpCommander(yaml_file_path)
    gps_wpf.start_wpf()


if __name__ == "__main__":
    main()

@ZaBy10
Copy link
Author

ZaBy10 commented Dec 18, 2023

Hi @LuukBerkel , Thanks for the reply and the solution......Appreciate it a lot!!!!

But after i change the code in logged_waypoint.py as mentioned above and run the following command :

ros2 run nav2_gps_waypoint_follower_demo logged_waypoint_follower

I am getting the following error :

[INFO] [1702907806.140409779] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907807.143284441] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907808.146023461] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907809.148571829] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907810.151336138] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907811.153767958] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907812.156675114] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907813.159296651] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907814.162433174] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907815.165633110] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907816.169049306] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907817.172883378] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907818.176170262] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907819.179552953] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907820.183124565] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907821.186571611] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907822.189306284] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907823.191735679] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907824.194469803] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907825.196974787] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907826.199480392] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907827.202489927] [basic_navigator]: robot_localization/get_state service not available, waiting...

But when I run : ros2 service list | grep robot_localization/get_state

The service is availabe

Could you please help me with this ??

@LuukBerkel
Copy link

LuukBerkel commented Dec 19, 2023

I compiled robot_localization from source from the ros2 branch at Robot Localization Github, instead of installing it using apt. I don't know if there is a big difference but could you try that.

@Abdul-Hannan-robo
Copy link

Hi @LuukBerkel , How did you install robot_localization from github??When i git clone https://github.com/cra-ros-pkg/robot_localization.git and then colcon build , I am getting the following error :

`Starting >>> robot_localization
--- stderr: robot_localization
CMake Error at CMakeLists.txt:39 (find_package):
By not providing "FindGeographicLib.cmake" in CMAKE_MODULE_PATH this
project has asked CMake to find a package configuration file provided by
"GeographicLib", but CMake did not find one.

Could not find a package configuration file provided by "GeographicLib"
with any of the following names:

GeographicLibConfig.cmake
geographiclib-config.cmake

Add the installation prefix of "GeographicLib" to CMAKE_PREFIX_PATH or set
"GeographicLib_DIR" to a directory containing one of the above files. If
"GeographicLib" provides a separate development package or SDK, be sure it
has been installed.


Failed <<< robot_localization [3.60s, exited with code 1]
`

@LuukBerkel
Copy link

Please install install libgeographic by running apt-get install libgeographic-dev. That is required library for robot_localization. If you install that and make sure you have the ros-humble-geographic-msgs installed you should be able to compile robot_localization.

@LuukBerkel
Copy link

LuukBerkel commented Dec 19, 2023

I have also created ported the interactive_waypoint_follower.py for the navigation demo. The code for that file this:

import rclpy
from rclpy.node import Node
from nav2_simple_commander.robot_navigator import BasicNavigator
from geometry_msgs.msg import PointStamped
from nav2_gps_waypoint_follower_demo.utils.gps_utils import latLonYaw2Geopose
from nav2_msgs.action import FollowWaypoints
from geometry_msgs.msg import PoseStamped
from robot_localization.srv import FromLL

class InteractiveGpsWpCommander(Node):
    """
    ROS2 node to send gps waypoints to nav2 received from mapviz's point click publisher
    """

    def __init__(self):
        super().__init__(node_name="gps_wp_commander")
        self.navigator = BasicNavigator("basic_navigator")
        self.mapviz_wp_sub = self.create_subscription(
            PointStamped, "/clicked_point", self.mapviz_wp_cb, 1)
        
        self.localizer = self.create_client(FromLL,  '/fromLL')
        while not self.localizer.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('Service not available, waiting again...')
        self.client_futures = []

        self.get_logger().info('Ready for waypoints...')

    def mapviz_wp_cb(self, msg: PointStamped):
        """
        clicked point callback, sends received point to nav2 gps waypoint follower if its a geographic point
        """
        if msg.header.frame_id != "wgs84":
            self.get_logger().warning(
                "Received point from mapviz that ist not in wgs84 frame. This is not a gps point and wont be followed")
            return
    
        wps = [latLonYaw2Geopose(msg.point.y, msg.point.x)]

        for wp in wps:
            self.req = FromLL.Request()
            self.req.ll_point.longitude = wp.position.longitude
            self.req.ll_point.latitude = wp.position.latitude
            self.req.ll_point.altitude = wp.position.altitude

            self.get_logger().info("Waypoint added to conversion queue...")
            self.client_futures.append(self.localizer.call_async(self.req))

    def command_send_cb(self, future):
        self.resp = PoseStamped()
        self.resp.header.frame_id = 'map'
        self.resp.header.stamp = self.get_clock().now().to_msg()
        self.resp.pose.position = future.result().map_point
    
        self.navigator.goToPose(self.resp)

    def spin(self):
        while rclpy.ok():
            rclpy.spin_once(self)
            incomplete_futures = []
            for f in self.client_futures:
                if f.done():
                    self.get_logger().info("Following converted waypoint...")
                    self.command_send_cb(f)
                else:
                    incomplete_futures.append(f)
                    
            self.client_futures = incomplete_futures

def main():
    rclpy.init()
    gps_wpf = InteractiveGpsWpCommander()
    gps_wpf.spin()


if __name__ == "__main__":
    main()

Here is a image of it working if you want to see it.
afbeelding

@ZaBy10
Copy link
Author

ZaBy10 commented Dec 21, 2023

I compiled robot_localization from source from the ros2 branch at Robot Localization Github, instead of installing it using apt. I don't know if there is a big difference but could you try that.

Hii @LuukBerkel , I tried doing what you suggested above ......But i am still getting the same error :

[INFO] [1702907806.140409779] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907807.143284441] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907808.146023461] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907809.148571829] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907810.151336138] [basic_navigator]: robot_localization/get_state service not available, waiting...

@LuukBerkel
Copy link

LuukBerkel commented Dec 21, 2023

I compiled robot_localization from source from the ros2 branch at Robot Localization Github, instead of installing it using apt. I don't know if there is a big difference but could you try that.

Hii @LuukBerkel , I tried doing what you suggested above ......But i am still getting the same error :

[INFO] [1702907806.140409779] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907807.143284441] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907808.146023461] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907809.148571829] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907810.151336138] [basic_navigator]: robot_localization/get_state service not available, waiting...

Did you source it. Just to be sure. Maybe you could try to remove the apt version. And i will check basic navigator of my self to check if i modified some thing. I will come back on.

@ZaBy10
Copy link
Author

ZaBy10 commented Dec 21, 2023

I compiled robot_localization from source from the ros2 branch at Robot Localization Github, instead of installing it using apt. I don't know if there is a big difference but could you try that.

Hii @LuukBerkel , I tried doing what you suggested above ......But i am still getting the same error :
[INFO] [1702907806.140409779] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907807.143284441] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907808.146023461] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907809.148571829] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907810.151336138] [basic_navigator]: robot_localization/get_state service not available, waiting...

Did you source it. Just to be sure. Maybe you could try to remove the apt version. And i will check basic navigator of my self to check if i modified some thing. I will come back on.

Yeah I removed the apt version of robot_localisation and sourced the workspace too.....

Thanks for the reply and take your time!!

@kevinpretell
Copy link

Hi @LuukBerkel @ZaBy10

When I do the following part of the tutorial:
ros2 launch nav2_gps_waypoint_follower_demo gps_waypoint_follower.launch.py use_rviz:=True

I get the following error:

[lifecycle_manager-14] [INFO] [1703237185.948767594] [lifecycle_manager_navigation]: Configuring bt_navigator
[bt_navigator-11] [INFO] [1703237185.949153207] [bt_navigator]: Configuring
[bt_navigator-11] [ERROR] [1703237186.011346833] [bt_navigator]: Caught exception in callback for transition 10
[bt_navigator-11] [ERROR] [1703237186.011417987] [bt_navigator]: Original error: Could not load library: libnav2_are_error_codes_active_condition_bt_node.so: cannot open shared object file: No such file or directory
[bt_navigator-11] [WARN] [1703237186.011465395] [bt_navigator]: Error occurred while doing error handling.
[bt_navigator-11] [FATAL] [1703237186.011483738] [bt_navigator]: Lifecycle node bt_navigator does not have error state implemented
[lifecycle_manager-14] [ERROR] [1703237186.012840641] [lifecycle_manager_navigation]: Failed to change state for node: bt_navigator
[lifecycle_manager-14] [ERROR] [1703237186.012889544] [lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.

As a result nav2 doesn't seem to start (it's inactive) and when I send a goal the robot doesn't move
Screenshot from 2023-12-22 10-34-50

how did you solve it?
thanks and best regards

@elgarbe
Copy link

elgarbe commented Jan 4, 2024

I'm trying this demo on humble too and have some error running ros2 launch nav2_gps_waypoint_follower_demo gps_waypoint_follower.launch.py use_rviz:=True :

[INFO] [launch]: All log files can be found below /home/elgarbe/.ros/log/2024-01-04-09-26-52-019959-elgarbe-9516
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [gzserver-1]: process started with pid [9517]
[INFO] [gzclient-2]: process started with pid [9519]
[INFO] [robot_state_publisher-3]: process started with pid [9521]
[INFO] [ekf_node-4]: process started with pid [9523]
[INFO] [ekf_node-5]: process started with pid [9525]
[INFO] [navsat_transform_node-6]: process started with pid [9527]
[INFO] [controller_server-7]: process started with pid [9529]
[INFO] [smoother_server-8]: process started with pid [9531]
[INFO] [planner_server-9]: process started with pid [9534]
[INFO] [behavior_server-10]: process started with pid [9538]
[INFO] [bt_navigator-11]: process started with pid [9545]
[INFO] [waypoint_follower-12]: process started with pid [9549]
[INFO] [velocity_smoother-13]: process started with pid [9555]
[INFO] [lifecycle_manager-14]: process started with pid [9558]
[INFO] [rviz2-15]: process started with pid [9562]
[robot_state_publisher-3] [INFO] [1704371212.618884101] [robot_state_publisher]: got segment base_footprint
[robot_state_publisher-3] [INFO] [1704371212.619055811] [robot_state_publisher]: got segment base_link
[robot_state_publisher-3] [INFO] [1704371212.619070752] [robot_state_publisher]: got segment base_scan
[robot_state_publisher-3] [INFO] [1704371212.619076291] [robot_state_publisher]: got segment camera_depth_frame
[robot_state_publisher-3] [INFO] [1704371212.619081494] [robot_state_publisher]: got segment camera_depth_optical_frame
[robot_state_publisher-3] [INFO] [1704371212.619086395] [robot_state_publisher]: got segment camera_link
[robot_state_publisher-3] [INFO] [1704371212.619091017] [robot_state_publisher]: got segment camera_rgb_frame
[robot_state_publisher-3] [INFO] [1704371212.619095646] [robot_state_publisher]: got segment camera_rgb_optical_frame
[robot_state_publisher-3] [INFO] [1704371212.619099910] [robot_state_publisher]: got segment caster_back_left_link
[robot_state_publisher-3] [INFO] [1704371212.619104531] [robot_state_publisher]: got segment caster_back_right_link
[robot_state_publisher-3] [INFO] [1704371212.619108567] [robot_state_publisher]: got segment gps_link
[robot_state_publisher-3] [INFO] [1704371212.619113298] [robot_state_publisher]: got segment imu_link
[robot_state_publisher-3] [INFO] [1704371212.619118822] [robot_state_publisher]: got segment wheel_left_link
[robot_state_publisher-3] [INFO] [1704371212.619123848] [robot_state_publisher]: got segment wheel_right_link
[controller_server-7] [INFO] [1704371212.630051084] [controller_server]: 
[controller_server-7] 	controller_server lifecycle node launched. 
[controller_server-7] 	Waiting on external lifecycle transitions to activate
[controller_server-7] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-7] [INFO] [1704371212.634654804] [controller_server]: Creating controller server
[smoother_server-8] [INFO] [1704371212.635789549] [smoother_server]: 
[smoother_server-8] 	smoother_server lifecycle node launched. 
[smoother_server-8] 	Waiting on external lifecycle transitions to activate
[smoother_server-8] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[smoother_server-8] [INFO] [1704371212.637445339] [smoother_server]: Creating smoother server
[waypoint_follower-12] [INFO] [1704371212.638345079] [waypoint_follower]: 
[waypoint_follower-12] 	waypoint_follower lifecycle node launched. 
[waypoint_follower-12] 	Waiting on external lifecycle transitions to activate
[waypoint_follower-12] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[waypoint_follower-12] [INFO] [1704371212.638816136] [waypoint_follower]: Creating
[navsat_transform_node-6] [WARN] [1704371212.639860583] [navsat_transform]: Parameter 'broadcast_utm_transform' has been deprecated. Please use 'broadcast_cartesian_transform' instead.
[rviz2-15] QStandardPaths: wrong permissions on runtime directory /run/user/1000/, 0755 instead of 0700
[lifecycle_manager-14] [INFO] [1704371212.653576067] [lifecycle_manager_navigation]: Creating
[lifecycle_manager-14] [INFO] [1704371212.657930972] [lifecycle_manager_navigation]: �[34m�[1mCreating and initializing lifecycle service clients�[0m�[0m
[bt_navigator-11] [INFO] [1704371212.659574130] [bt_navigator]: 
[bt_navigator-11] 	bt_navigator lifecycle node launched. 
[bt_navigator-11] 	Waiting on external lifecycle transitions to activate
[bt_navigator-11] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-11] [INFO] [1704371212.659748913] [bt_navigator]: Creating
[controller_server-7] [INFO] [1704371212.670900098] [local_costmap.local_costmap]: 
[controller_server-7] 	local_costmap lifecycle node launched. 
[controller_server-7] 	Waiting on external lifecycle transitions to activate
[controller_server-7] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-7] [INFO] [1704371212.671943548] [local_costmap.local_costmap]: Creating Costmap
[behavior_server-10] [INFO] [1704371212.672488417] [behavior_server]: 
[behavior_server-10] 	behavior_server lifecycle node launched. 
[behavior_server-10] 	Waiting on external lifecycle transitions to activate
[behavior_server-10] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[velocity_smoother-13] [INFO] [1704371212.673725685] [velocity_smoother]: 
[velocity_smoother-13] 	velocity_smoother lifecycle node launched. 
[velocity_smoother-13] 	Waiting on external lifecycle transitions to activate
[velocity_smoother-13] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-9] [INFO] [1704371212.689440133] [planner_server]: 
[planner_server-9] 	planner_server lifecycle node launched. 
[planner_server-9] 	Waiting on external lifecycle transitions to activate
[planner_server-9] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-9] [INFO] [1704371212.692465933] [planner_server]: Creating
[planner_server-9] [INFO] [1704371212.718788918] [global_costmap.global_costmap]: 
[planner_server-9] 	global_costmap lifecycle node launched. 
[planner_server-9] 	Waiting on external lifecycle transitions to activate
[planner_server-9] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-9] [INFO] [1704371212.719444404] [global_costmap.global_costmap]: Creating Costmap
[lifecycle_manager-14] [INFO] [1704371213.688509187] [lifecycle_manager_navigation]: �[34m�[1mStarting managed nodes bringup...�[0m�[0m
[lifecycle_manager-14] [INFO] [1704371213.688583975] [lifecycle_manager_navigation]: �[34m�[1mConfiguring controller_server�[0m�[0m
[controller_server-7] [INFO] [1704371213.688933146] [controller_server]: Configuring controller interface
[controller_server-7] [INFO] [1704371213.689424741] [controller_server]: getting goal checker plugins..
[controller_server-7] [INFO] [1704371213.689904935] [controller_server]: Controller frequency set to 20.0000Hz
[controller_server-7] [INFO] [1704371213.689983642] [local_costmap.local_costmap]: Configuring
[controller_server-7] [INFO] [1704371213.700739792] [local_costmap.local_costmap]: Using plugin "voxel_layer"
[controller_server-7] [INFO] [1704371213.707012201] [local_costmap.local_costmap]: Subscribed to Topics: scan
[controller_server-7] [INFO] [1704371213.720772289] [local_costmap.local_costmap]: Initialized plugin "voxel_layer"
[controller_server-7] [INFO] [1704371213.720833285] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-7] [INFO] [1704371213.723340980] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[controller_server-7] [INFO] [1704371213.740356972] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-7] [INFO] [1704371213.742720983] [controller_server]: Created goal checker : general_goal_checker of type nav2_controller::SimpleGoalChecker
[controller_server-7] [INFO] [1704371213.744267881] [controller_server]: Controller Server has general_goal_checker  goal checkers available.
[controller_server-7] [INFO] [1704371213.745748687] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[controller_server-7] [INFO] [1704371213.748456705] [controller_server]: Setting transform_tolerance to 0.200000
[controller_server-7] [INFO] [1704371213.765824613] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[controller_server-7] [INFO] [1704371213.766991173] [controller_server]: Critic plugin initialized
[controller_server-7] [INFO] [1704371213.767585172] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[controller_server-7] [INFO] [1704371213.768677659] [controller_server]: Critic plugin initialized
[controller_server-7] [INFO] [1704371213.768948044] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[controller_server-7] [INFO] [1704371213.769367471] [controller_server]: Critic plugin initialized
[controller_server-7] [INFO] [1704371213.769578927] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[controller_server-7] [INFO] [1704371213.770252688] [controller_server]: Critic plugin initialized
[controller_server-7] [INFO] [1704371213.770514302] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[controller_server-7] [INFO] [1704371213.771177341] [controller_server]: Critic plugin initialized
[controller_server-7] [INFO] [1704371213.771432663] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[controller_server-7] [INFO] [1704371213.771883748] [controller_server]: Critic plugin initialized
[controller_server-7] [INFO] [1704371213.772128482] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[controller_server-7] [INFO] [1704371213.772546581] [controller_server]: Critic plugin initialized
[controller_server-7] [INFO] [1704371213.772573101] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-14] [INFO] [1704371213.782461270] [lifecycle_manager_navigation]: �[34m�[1mConfiguring smoother_server�[0m�[0m
[smoother_server-8] [INFO] [1704371213.782719922] [smoother_server]: Configuring smoother server
[smoother_server-8] [INFO] [1704371213.792459681] [smoother_server]: Created smoother : simple_smoother of type nav2_smoother::SimpleSmoother
[smoother_server-8] [INFO] [1704371213.794020798] [smoother_server]: Smoother Server has simple_smoother  smoothers available.
[lifecycle_manager-14] [INFO] [1704371213.802028270] [lifecycle_manager_navigation]: �[34m�[1mConfiguring planner_server�[0m�[0m
[planner_server-9] [INFO] [1704371213.802300858] [planner_server]: Configuring
[planner_server-9] [INFO] [1704371213.802332111] [global_costmap.global_costmap]: Configuring
[planner_server-9] [INFO] [1704371213.808577123] [global_costmap.global_costmap]: Using plugin "obstacle_layer"
[planner_server-9] [INFO] [1704371213.812101040] [global_costmap.global_costmap]: Subscribed to Topics: scan
[planner_server-9] [INFO] [1704371213.817479788] [global_costmap.global_costmap]: Initialized plugin "obstacle_layer"
[planner_server-9] [INFO] [1704371213.817533762] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-9] [INFO] [1704371213.819082585] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-9] [INFO] [1704371213.831335462] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner
[planner_server-9] [INFO] [1704371213.831392767] [planner_server]: Configuring plugin GridBased of type NavfnPlanner
[planner_server-9] [INFO] [1704371213.833087431] [planner_server]: Planner Server has GridBased  planners available.
[lifecycle_manager-14] [INFO] [1704371213.845449910] [lifecycle_manager_navigation]: �[34m�[1mConfiguring behavior_server�[0m�[0m
[behavior_server-10] [INFO] [1704371213.845805869] [behavior_server]: Configuring
[behavior_server-10] [INFO] [1704371213.855707010] [behavior_server]: Creating behavior plugin spin of type nav2_behaviors/Spin
[behavior_server-10] [INFO] [1704371213.856328167] [behavior_server]: Configuring spin
[behavior_server-10] [INFO] [1704371213.864677881] [behavior_server]: Creating behavior plugin backup of type nav2_behaviors/BackUp
[behavior_server-10] [INFO] [1704371213.865316928] [behavior_server]: Configuring backup
[behavior_server-10] [INFO] [1704371213.872055674] [behavior_server]: Creating behavior plugin drive_on_heading of type nav2_behaviors/DriveOnHeading
[behavior_server-10] [INFO] [1704371213.872691094] [behavior_server]: Configuring drive_on_heading
[behavior_server-10] [INFO] [1704371213.879419663] [behavior_server]: Creating behavior plugin assisted_teleop of type nav2_behaviors/AssistedTeleop
[behavior_server-10] [INFO] [1704371213.881258630] [behavior_server]: Configuring assisted_teleop
[behavior_server-10] [INFO] [1704371213.889958056] [behavior_server]: Creating behavior plugin wait of type nav2_behaviors/Wait
[behavior_server-10] [INFO] [1704371213.890510876] [behavior_server]: Configuring wait
[lifecycle_manager-14] [INFO] [1704371213.897822459] [lifecycle_manager_navigation]: �[34m�[1mConfiguring bt_navigator�[0m�[0m
[bt_navigator-11] [INFO] [1704371213.898111695] [bt_navigator]: Configuring
[bt_navigator-11] [ERROR] [1704371213.935205497] [bt_navigator]: Caught exception in callback for transition 10
[bt_navigator-11] [ERROR] [1704371213.935271411] [bt_navigator]: Original error: Could not load library: libnav2_are_error_codes_active_condition_bt_node.so: cannot open shared object file: No such file or directory
[bt_navigator-11] [WARN] [1704371213.935291798] [bt_navigator]: Error occurred while doing error handling.
[bt_navigator-11] [FATAL] [1704371213.935300009] [bt_navigator]: Lifecycle node bt_navigator does not have error state implemented
[lifecycle_manager-14] [ERROR] [1704371213.935891341] [lifecycle_manager_navigation]: Failed to change state for node: bt_navigator
[lifecycle_manager-14] [ERROR] [1704371213.935969962] [lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.
[rviz2-15] [INFO] [1704371215.212233248] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-15] [INFO] [1704371215.212353476] [rviz2]: OpenGl version: 4.1 (GLSL 4.1)
[rviz2-15] [INFO] [1704371215.244885143] [rviz2]: Stereo is NOT SUPPORTED
[gzserver-1] [INFO] [1704371216.389552019] [intel_realsense_r200_depth_driver]: Publishing camera info to [/intel_realsense_r200_depth/camera_info]
[gzserver-1] [INFO] [1704371216.393088947] [intel_realsense_r200_depth_driver]: Publishing depth camera info to [/intel_realsense_r200_depth/depth/camera_info]
[gzserver-1] [INFO] [1704371216.394478925] [intel_realsense_r200_depth_driver]: Publishing pointcloud to [/intel_realsense_r200_depth/points]
[gzserver-1] [INFO] [1704371216.439332438] [turtlebot3_diff_drive]: Wheel pair 1 separation set to [0.287000m]
[gzserver-1] [INFO] [1704371216.439504648] [turtlebot3_diff_drive]: Wheel pair 1 diameter set to [0.066000m]
[gzserver-1] [INFO] [1704371216.443774011] [turtlebot3_diff_drive]: Subscribed to [/cmd_vel]
[gzserver-1] [INFO] [1704371216.448348597] [turtlebot3_diff_drive]: Advertise odometry on [/odom]
[gzserver-1] [INFO] [1704371216.478154271] [turtlebot3_joint_state]: Going to publish joint [wheel_left_joint]
[gzserver-1] [INFO] [1704371216.478262524] [turtlebot3_joint_state]: Going to publish joint [wheel_right_joint]
[navsat_transform_node-6] [INFO] [1704371217.481496735] [navsat_transform]: Datum (latitude, longitude, altitude) is (38.16, -122.45, 488.30)
[navsat_transform_node-6] [INFO] [1704371217.481592956] [navsat_transform]: Datum UTM coordinate is (10S, 547779.04, 4223869.70)
[navsat_transform_node-6] [INFO] [1704371217.488108682] [navsat_transform]: Corrected for magnetic declination of 0, user-specified offset of 0 and meridian convergence of 0.00588164. Transform heading factor is now 0.00441123
[rviz2-15] Start navigation
[rviz2-15] [INFO] [1704371232.038018627] [rviz_navigation_dialog_action_client]: NavigateToPose will be called using the BT Navigator's default behavior tree.
[rviz2-15] [ERROR] [1704371232.039746976] [rviz_navigation_dialog_action_client]: Goal was rejected by server

is this errors related to humble vs iron distro?
is there a workaround for this?

Thank

@ZaBy10
Copy link
Author

ZaBy10 commented Jan 17, 2024

I compiled robot_localization from source from the ros2 branch at Robot Localization Github, instead of installing it using apt. I don't know if there is a big difference but could you try that.

Hii @LuukBerkel , I tried doing what you suggested above ......But i am still getting the same error :
[INFO] [1702907806.140409779] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907807.143284441] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907808.146023461] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907809.148571829] [basic_navigator]: robot_localization/get_state service not available, waiting... [INFO] [1702907810.151336138] [basic_navigator]: robot_localization/get_state service not available, waiting...

Did you source it. Just to be sure. Maybe you could try to remove the apt version. And i will check basic navigator of my self to check if i modified some thing. I will come back on.

Hii @LuukBerkel , Any updates regarding this??

@ghoenicka
Copy link

ghoenicka commented Jan 25, 2024

cra-ros-pkg/robot_localization#844

I was able to run the demo after the solution of @LuukBerkel. Thanks.

Now.. the only problem is the all values transformed are '0'

[INFO] [1706183873.940299133] [basic_navigator]: Nav2 is ready for use!
[INFO] [1706183873.940979432] [minimal_client_async]: long-122.454572, lat=38.161461, alt=0.000000
[INFO] [1706183873.942186002] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.942651190] [minimal_client_async]: long-122.454539, lat=38.161437, alt=0.000000
[INFO] [1706183873.943659020] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.944206872] [minimal_client_async]: long-122.454485, lat=38.161397, alt=0.000000
[INFO] [1706183873.945213550] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.945664181] [minimal_client_async]: long-122.454410, lat=38.161352, alt=0.000000
[INFO] [1706183873.946627438] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.947060656] [minimal_client_async]: long-122.454300, lat=38.161282, alt=0.000000
[INFO] [1706183873.947931711] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.948325145] [minimal_client_async]: long-122.454189, lat=38.161213, alt=0.000000
[INFO] [1706183873.949087086] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.949466534] [minimal_client_async]: long-122.454005, lat=38.161099, alt=0.000000
[INFO] [1706183873.950333661] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.950701467] [minimal_client_async]: long-122.453699, lat=38.160904, alt=0.000000
[INFO] [1706183873.951485951] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.951916093] [minimal_client_async]: long-122.453415, lat=38.160643, alt=0.000000
[INFO] [1706183873.952997119] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.953522239] [minimal_client_async]: long-122.453336, lat=38.160487, alt=0.000000
[INFO] [1706183873.954733428] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.955380455] [minimal_client_async]: long-122.453356, lat=38.160438, alt=0.000000
[INFO] [1706183873.956687092] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.957174552] [minimal_client_async]: long-122.453448, lat=38.160513, alt=0.000000
[INFO] [1706183873.958092985] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.958485908] [minimal_client_async]: long-122.453484, lat=38.160537, alt=0.000000
[INFO] [1706183873.959598112] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.960389719] [basic_navigator]: Following 13 goals....
wps completed successfully

I tested the 'interactive_waypoint_follower' modification and I have the same result:

gazebo_simulation$ ros2 run nav2_gps_waypoint_follower_demo interactive_waypoint_follower 
[INFO] [1706184687.489877331] [gps_wp_commander]: Ready for waypoints...
[INFO] [1706184694.186810604] [gps_wp_commander]: Waypoint added to conversion queue...
[INFO] [1706184694.187833733] [gps_wp_commander]: Following converted waypoint...
[INFO] [1706184694.188709537] [basic_navigator]: Navigating to goal: 0.0 0.0...
[INFO] [1706184719.943411675] [gps_wp_commander]: Waypoint added to conversion queue...
[INFO] [1706184719.945123148] [gps_wp_commander]: Following converted waypoint...
[INFO] [1706184719.945930724] [basic_navigator]: Navigating to goal: 0.0 0.0...
[INFO] [1706184788.993477721] [gps_wp_commander]: Waypoint added to conversion queue...
[INFO] [1706184788.994500629] [gps_wp_commander]: Following converted waypoint...
[INFO] [1706184788.995235591] [basic_navigator]: Navigating to goal: 0.0 0.0...

I'm using HUMBLE distro and Nav2 from apt installation..

@ZaBy10
Copy link
Author

ZaBy10 commented Jan 26, 2024

Hii @ghoenicka Could you please tell me how did you achieve this . What all changes did you make?

Did you just change the logged_waypoint_follower.py ?

Please help me !!!

@elgarbe
Copy link

elgarbe commented Jan 26, 2024

Cant' you just install ros iron? It works pretty well on it. Or even using a docker container? I'm doing that waiting for a solution on Humble ...

@ZaBy10
Copy link
Author

ZaBy10 commented Jan 26, 2024

I tried using the docker but found no success

@elgarbe
Copy link

elgarbe commented Jan 26, 2024

I tried using the docker but found no success

Just in case you want to give it a try again:
Dockerfile:

FROM osrf/ros:iron-desktop

WORKDIR /home/nav2_gps_ws

RUN . /opt/ros/iron/setup.sh \
    && apt-get update \
    && apt-get install -y python3-pip \
    ros-iron-demo-nodes-cpp \
    ros-iron-robot-localization \
    ros-iron-mapviz \
    ros-iron-mapviz-plugins \
    ros-iron-tile-map \
    ros-iron-navigation2 \
    ros-iron-nav2-bringup \
    ros-iron-turtlebot3-gazebo \
    &&  apt autoremove -y \
    &&  rm -rf /var/lib/apt/lists/*

RUN git clone https://github.com/ros-planning/navigation2_tutorials.git src/navigation2_tutorials \
    && . /opt/ros/iron/setup.sh \
    && apt-get update \
    && rosdep update \
    && rosdep install --from-paths src --ignore-src -y \
    && colcon build \
    && rm -rf log/ build/ src/* \
    && rm -rf /var/lib/apt/lists/*

COPY ros_entrypoint.sh /
ENTRYPOINT [ "/ros_entrypoint.sh" ]

CMD ["bash"]

ros_entrypoint.sh

#!/bin/bash
 
# Source ROS and Catkin workspaces
source /opt/ros/iron/setup.bash
if [ -f /home/nav2_gps_ws/install/setup.bash ]
then
  source /home/nav2_gps_ws/install/setup.bash
  echo "Sourced nav2_gps workspace!"
fi
 
# Set environment variables
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/iron/share/turtlebot3_gazebo/models

# Execute the command passed into this entrypoint
exec "$@"

put the files on a folder and run:
docker build . -t nav2_gps_docker

After it finished you can run:
docker run -it --rm --net host --env="NVIDIA_DRIVER_CAPABILITIES=all" --env="DISPLAY" --env="QT_X11_NO_MITSHM=1" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" nav2_gps_docker:latest /bin/bash
take a llok a some env options I need to use my nvidia GPU, maybe you don't need them.
Once inside the container run:
ros2 launch nav2_gps_waypoint_follower_demo gps_waypoint_follower.launch.py use_rviz:=True
give it some minutes to this because gazebo needs to download some models, I think.
Once gazebo shows up you can open another terminal and run:
docker ps
copy CONTAINER ID and run:
docker exec -it CONTAINER ID /bin/bash
to get a terminal on the container and run:

source install/setup.bash
ros2 run nav2_gps_waypoint_follower_demo logged_waypoint_follower

it will start 4 WP stored on the demo yaml file.

BTW I'm on Windows 11, running ubuntu 22.04 on WSL and inside it using Docker.
Once you have this working you could try a dev container in order to develop your own code.

test_nav2_gps

Hope this help!

@ghoenicka
Copy link

Hii @ghoenicka Could you please tell me how did you achieve this . What all changes did you make?

Did you just change the logged_waypoint_follower.py ?

Please help me !!!

Hi @ZaBy10 .. I been followed this official tutorial from Nav2 (https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html) with the only difference is I have a custom robot (not the turtle bot).

When I reached the GPS navigation, I found the problem with the difference between HUMBLE and IRON ( explained here: cra-ros-pkg/robot_localization#844 ) , I do the changes on my distro files ( /opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py ) and finally found this thread with the problem on followGpsWaypoints(wps)... I applied the changes on interactive_waypoint_follower and logged_waypoint_follower. .. It's start working !! .. but.. with the problem of the all values returned are '0' .. but I think that is a problem with my frames of references.. (still working to find this solution)

try to see the logs of the ROS every time that you run some launch.. because I noticed: if there is a problem for example loading the gazebo, you will not have any problem when is running but you will see only a small line with the tag 'ERROR'.. that's happens to me a loot. Bad parameters ..

@ZaBy10
Copy link
Author

ZaBy10 commented Jan 29, 2024

Hii @ghoenicka Could you please tell me how did you achieve this . What all changes did you make?
Did you just change the logged_waypoint_follower.py ?
Please help me !!!

Hi @ZaBy10 .. I been followed this official tutorial from Nav2 (https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html) with the only difference is I have a custom robot (not the turtle bot).

When I reached the GPS navigation, I found the problem with the difference between HUMBLE and IRON ( explained here: cra-ros-pkg/robot_localization#844 ) , I do the changes on my distro files ( /opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py ) and finally found this thread with the problem on followGpsWaypoints(wps)... I applied the changes on interactive_waypoint_follower and logged_waypoint_follower. .. It's start working !! .. but.. with the problem of the all values returned are '0' .. but I think that is a problem with my frames of references.. (still working to find this solution)

try to see the logs of the ROS every time that you run some launch.. because I noticed: if there is a problem for example loading the gazebo, you will not have any problem when is running but you will see only a small line with the tag 'ERROR'.. that's happens to me a loot. Bad parameters ..

Hii @ghoenicka , Thanks for the reply , I would like to know what changes did you make in the following file : (/opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py )

@ghoenicka
Copy link

ghoenicka commented Jan 29, 2024

Hii @ghoenicka Could you please tell me how did you achieve this . What all changes did you make?
Did you just change the logged_waypoint_follower.py ?
Please help me !!!

Hi @ZaBy10 .. I been followed this official tutorial from Nav2 (https://navigation.ros.org/tutorials/docs/navigation2_with_gps.html) with the only difference is I have a custom robot (not the turtle bot).
When I reached the GPS navigation, I found the problem with the difference between HUMBLE and IRON ( explained here: cra-ros-pkg/robot_localization#844 ) , I do the changes on my distro files ( /opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py ) and finally found this thread with the problem on followGpsWaypoints(wps)... I applied the changes on interactive_waypoint_follower and logged_waypoint_follower. .. It's start working !! .. but.. with the problem of the all values returned are '0' .. but I think that is a problem with my frames of references.. (still working to find this solution)
try to see the logs of the ROS every time that you run some launch.. because I noticed: if there is a problem for example loading the gazebo, you will not have any problem when is running but you will see only a small line with the tag 'ERROR'.. that's happens to me a loot. Bad parameters ..

Hii @ghoenicka , Thanks for the reply , I would like to know what changes did you make in the following file : (/opt/ros/humble/lib/python3.10/site-packages/nav2_simple_commander/robot_navigator.py )

    def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
        """Block until the full navigation system is up and running."""
        # TODO: commented. https://github.com/cra-ros-pkg/robot_localization/issues/844
        # self._waitForNodeToActivate(localizer)
        if localizer != "robot_localization":
            self._waitForNodeToActivate(localizer)
        if localizer == 'amcl':
            self._waitForInitialPose()
        self._waitForNodeToActivate(navigator)
        self.info('Nav2 is ready for use!')
        return

Additionally I've made change on the file: nav2_no_map_params.yaml

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: odom
    bt_loop_duration: 10
    default_server_timeout: 20
    navigators: ["navigate_to_pose", "navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
    plugin_lib_names:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_smooth_path_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_assisted_teleop_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_drive_on_heading_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_globally_updated_goal_condition_bt_node
    - nav2_is_path_valid_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_truncate_path_local_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_path_expiring_timer_condition
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_goal_updated_controller_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node
    - nav2_controller_cancel_bt_node
    - nav2_path_longer_on_approach_bt_node
    - nav2_wait_cancel_bt_node
    - nav2_spin_cancel_bt_node
    - nav2_back_up_cancel_bt_node
    - nav2_assisted_teleop_cancel_bt_node
    - nav2_drive_on_heading_cancel_bt_node
    - nav2_is_battery_charging_condition_bt_node    
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

PS: I fixed my problem with the transformation of the GPS to MAP points.. was a bad definition of the robot frame.

@ZaBy10
Copy link
Author

ZaBy10 commented Jan 29, 2024

Hii @ghoenicka , I have changed the no_map_params.yaml and logged_waypoint_follower.py files in both install and the src directories of my package . After running the command : ros2 run nav2_gps_waypoint_follower_demo logged_waypoint_follower /home/zaby/gps_waypoints.yaml

I am getting the following error :

`Traceback (most recent call last):
File "/opt/ros/humble/lib/python3.10/site-packages/ament_index_python/packages.py", line 58, in get_package_prefix
content, package_prefix = get_resource('packages', package_name)
File "/opt/ros/humble/lib/python3.10/site-packages/ament_index_python/resources.py", line 86, in get_resource
raise LookupError(
LookupError: Could not find the resource 'node' of type 'packages'

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
File "/home/zaby/gps_waypoint_follower/install/nav2_gps_waypoint_follower_demo/lib/nav2_gps_waypoint_follower_demo/logged_waypoint_follower", line 33, in
sys.exit(load_entry_point('nav2-gps-waypoint-follower-demo==0.0.1', 'console_scripts', 'logged_waypoint_follower')())
File "/home/zaby/gps_waypoint_follower/install/nav2_gps_waypoint_follower_demo/lib/python3.10/site-packages/nav2_gps_waypoint_follower_demo/logged_waypoint_follower.py", line 86, in main
default_yaml_file_path = os.path.join(get_package_share_directory(
File "/opt/ros/humble/lib/python3.10/site-packages/ament_index_python/packages.py", line 80, in get_package_share_directory
path = os.path.join(get_package_prefix(package_name), 'share', package_name)
File "/opt/ros/humble/lib/python3.10/site-packages/ament_index_python/packages.py", line 60, in get_package_prefix
raise PackageNotFoundError(
ament_index_python.packages.PackageNotFoundError: "package 'node' not found, searching: ['/home/zaby/gps_waypoint_follower/install/nav2_gps_waypoint_follower_demo', '/home/zaby/turtlebot3_ws/install/turtlebot3', '/home/zaby/turtlebot3_ws/install/turtlebot3_teleop', '/home/zaby/turtlebot3_ws/install/turtlebot3_simulations', '/home/zaby/turtlebot3_ws/install/turtlebot3_bringup', '/home/zaby/turtlebot3_ws/install/turtlebot3_node', '/home/zaby/turtlebot3_ws/install/turtlebot3_navigation2', '/home/zaby/turtlebot3_ws/install/turtlebot3_fake_node', '/home/zaby/turtlebot3_ws/install/turtlebot3_example', '/home/zaby/turtlebot3_ws/install/turtlebot3_msgs', '/home/zaby/turtlebot3_ws/install/turtlebot3_gazebo', '/home/zaby/turtlebot3_ws/install/turtlebot3_description', '/home/zaby/turtlebot3_ws/install/turtlebot3_cartographer', '/home/zaby/turtlebot3_ws/install/robot_pose_publisher_ros2', '/home/zaby/turtlebot3_ws/install/dynamixel_sdk_examples', '/home/zaby/turtlebot3_ws/install/dynamixel_sdk_custom_interfaces', '/home/zaby/turtlebot3_ws/install/dynamixel_sdk', '/opt/ros/humble']"
[ros2run]: Process exited with failure 1`

@ZaBy10 ZaBy10 closed this as completed Jan 29, 2024
@ZaBy10 ZaBy10 reopened this Jan 29, 2024
@ghoenicka
Copy link

ghoenicka commented Jan 30, 2024

In the function main, replace the word "node" by the name of your package. (example: "my_robot_gps")

def main():
    rclpy.init()

    # allow to pass the waypoints file as an argument
    default_yaml_file_path = os.path.join(get_package_share_directory(
        "node"), "config", "demo_waypoints.yaml")
    if len(sys.argv) > 1:
        yaml_file_path = sys.argv[1]
    else:
        yaml_file_path = default_yaml_file_path

    gps_wpf = GpsWpCommander(yaml_file_path)
    gps_wpf.start_wpf()

@ZaBy10
Copy link
Author

ZaBy10 commented Jan 30, 2024

Thanks a lot @LuukBerkel @ghoenicka , it finally worked !!!

@ghoenicka is it working on a real robot?

@ghoenicka
Copy link

Thanks a lot @LuukBerkel @ghoenicka , it finally worked !!!

@ghoenicka is it working on a real robot?

I will tested on a real one by March. I hope that will work with out problem.

@Ming2zun
Copy link

你好

当我执行教程的以下部分时: ros2 launch nav2_gps_waypoint_follower_demo gps_waypoint_follower.launch.py use_rviz:=True

我收到以下错误:

[lifecycle_manager-14] [INFO] [1703237185.948767594] [lifecycle_manager_navigation]: Configuring bt_navigator
[bt_navigator-11] [INFO] [1703237185.949153207] [bt_navigator]: Configuring
[bt_navigator-11] [ERROR] [1703237186.011346833] [bt_navigator]: Caught exception in callback for transition 10
[bt_navigator-11] [ERROR] [1703237186.011417987] [bt_navigator]: Original error: Could not load library: libnav2_are_error_codes_active_condition_bt_node.so: cannot open shared object file: No such file or directory
[bt_navigator-11] [WARN] [1703237186.011465395] [bt_navigator]: Error occurred while doing error handling.
[bt_navigator-11] [FATAL] [1703237186.011483738] [bt_navigator]: Lifecycle node bt_navigator does not have error state implemented
[lifecycle_manager-14] [ERROR] [1703237186.012840641] [lifecycle_manager_navigation]: Failed to change state for node: bt_navigator
[lifecycle_manager-14] [ERROR] [1703237186.012889544] [lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.

结果,nav2 似乎没有启动(它处于非活动状态),当我发送目标时,机器人不会移动 Screenshot from 2023-12-22 10-34-50

你是怎么解决的?谢谢和最诚挚的问候

I have solved this problem on humble. You should pay attention to the plugin _ lib _ names in this file nav2_no_map_prams.yaml: lines 32, 41, 42, 43, 44, 61 and 76. Just comment out these nodes.
Uploading 121212.png…

@emredemiroz1
Copy link

Thanks a lot @LuukBerkel @ghoenicka , it finally worked !!!

@ghoenicka is it working on a real robot?

Hello, I am having the same problem as you. Could you please explain step by step how you found the solution for Ros2 humble?

@Ming2zun
Copy link

Ming2zun commented Mar 22, 2024 via email

@ErikParkerrr
Copy link

hi all,

Clearly this is a very in demand feature. devs, would you consider properly back porting this to humble?

I'm running this on a Jetson and I'm running into issues with not every package being supported on aarch64/arm64 for iron. A backport would make life much easier.

Thanks

@ZaBy10
Copy link
Author

ZaBy10 commented May 23, 2024

Thanks a lot @LuukBerkel @ghoenicka , it finally worked !!!
@ghoenicka is it working on a real robot?

I will tested on a real one by March. I hope that will work with out problem.

Hii @ghoenicka , Did You test it on a real robot ?? what was the result , please share!!!

@ghoenicka
Copy link

Thanks a lot @LuukBerkel @ghoenicka , it finally worked !!!
@ghoenicka is it working on a real robot?

I will tested on a real one by March. I hope that will work with out problem.

Hii @ghoenicka , Did You test it on a real robot ?? what was the result , please share!!!

Hey.. not yet.. we made some changes in the plans.. but we hope at end of this year see it running in the first prototype..

@yishaiSilver
Copy link

Now.. the only problem is the all values transformed are '0'

This is probably because you're unable to transform GPS coordinates to your local coordinate system. Are you sure that your GPS localization system is up and running? I.e. can you get the transform from UTM to baselink?

@ghoenicka
Copy link

Now.. the only problem is the all values transformed are '0'

This is probably because you're unable to transform GPS coordinates to your local coordinate system. Are you sure that your GPS localization system is up and running? I.e. can you get the transform from UTM to baselink?

Hi @yishaiSilver .. I already fixed this. was a problem with the system definition of my robot between gps frame and the base frame.

@yishaiSilver
Copy link

yishaiSilver commented May 31, 2024

@LuukBerkel Your code seems to be working great, thank you. That being the case, I seem to be having trouble with the ToLL service, despite it working with terminal. Is there something I'm missing (asides from obvious changes, i.e. FromLL -> ToLL)?

Update: Nevermind, I was spinning my node with rclpy.spin(node_name), not node_name.spin()

@yishaiSilver
Copy link

Just writing that I think the spin() function from LuukBerkel incorrectly calls self.client_futures.remove(f). This causes an issue with loop iteration that prevents all requests from being iterated over. Instead, just don't add the future to incomplete_futures (as is already the case).

Otherwise the code looks great.

@ZainCDEI
Copy link

ZainCDEI commented Jul 4, 2024

cra-ros-pkg/robot_localization#844

I was able to run the demo after the solution of @LuukBerkel. Thanks.

Now.. the only problem is the all values transformed are '0'

[INFO] [1706183873.940299133] [basic_navigator]: Nav2 is ready for use!
[INFO] [1706183873.940979432] [minimal_client_async]: long-122.454572, lat=38.161461, alt=0.000000
[INFO] [1706183873.942186002] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.942651190] [minimal_client_async]: long-122.454539, lat=38.161437, alt=0.000000
[INFO] [1706183873.943659020] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.944206872] [minimal_client_async]: long-122.454485, lat=38.161397, alt=0.000000
[INFO] [1706183873.945213550] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.945664181] [minimal_client_async]: long-122.454410, lat=38.161352, alt=0.000000
[INFO] [1706183873.946627438] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.947060656] [minimal_client_async]: long-122.454300, lat=38.161282, alt=0.000000
[INFO] [1706183873.947931711] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.948325145] [minimal_client_async]: long-122.454189, lat=38.161213, alt=0.000000
[INFO] [1706183873.949087086] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.949466534] [minimal_client_async]: long-122.454005, lat=38.161099, alt=0.000000
[INFO] [1706183873.950333661] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.950701467] [minimal_client_async]: long-122.453699, lat=38.160904, alt=0.000000
[INFO] [1706183873.951485951] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.951916093] [minimal_client_async]: long-122.453415, lat=38.160643, alt=0.000000
[INFO] [1706183873.952997119] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.953522239] [minimal_client_async]: long-122.453336, lat=38.160487, alt=0.000000
[INFO] [1706183873.954733428] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.955380455] [minimal_client_async]: long-122.453356, lat=38.160438, alt=0.000000
[INFO] [1706183873.956687092] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.957174552] [minimal_client_async]: long-122.453448, lat=38.160513, alt=0.000000
[INFO] [1706183873.958092985] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.958485908] [minimal_client_async]: long-122.453484, lat=38.160537, alt=0.000000
[INFO] [1706183873.959598112] [minimal_client_async]: x=0.000000, y=0.000000, z=0.000000
[INFO] [1706183873.960389719] [basic_navigator]: Following 13 goals....
wps completed successfully

I tested the 'interactive_waypoint_follower' modification and I have the same result:

gazebo_simulation$ ros2 run nav2_gps_waypoint_follower_demo interactive_waypoint_follower 
[INFO] [1706184687.489877331] [gps_wp_commander]: Ready for waypoints...
[INFO] [1706184694.186810604] [gps_wp_commander]: Waypoint added to conversion queue...
[INFO] [1706184694.187833733] [gps_wp_commander]: Following converted waypoint...
[INFO] [1706184694.188709537] [basic_navigator]: Navigating to goal: 0.0 0.0...
[INFO] [1706184719.943411675] [gps_wp_commander]: Waypoint added to conversion queue...
[INFO] [1706184719.945123148] [gps_wp_commander]: Following converted waypoint...
[INFO] [1706184719.945930724] [basic_navigator]: Navigating to goal: 0.0 0.0...
[INFO] [1706184788.993477721] [gps_wp_commander]: Waypoint added to conversion queue...
[INFO] [1706184788.994500629] [gps_wp_commander]: Following converted waypoint...
[INFO] [1706184788.995235591] [basic_navigator]: Navigating to goal: 0.0 0.0...

I'm using HUMBLE distro and Nav2 from apt installation..

Hi @ghoenicka,

How did you solve for having all poses set to (0,0,0)? I have run into the same problem that the GPS point I provide is always converted to (0,0,0) position value and thus the robot doesn't move.

@JCastel358
Copy link

PS: I fixed my problem with the transformation of the GPS to MAP points.. was a bad definition of the robot frame.

Hey @ghoenicka I hope all is well! I noticed I'm getting the same erro, zeros for map frame. could you expand on "bad definition of the robot frame" and what it was that you fixed ? thanks!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests