nav2_gps_waypoint_follower_demo does not work on ros2 humble???! #77
Comments
|
The newest version of nav2, contains the function 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 followsDo the conversion yourself from Geopose to PoseStamped using robot_localization and then call the 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() |
|
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 :
I am getting the following error :
But when I run : The service is availabe Could you please help me with this ?? |
|
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. |
|
Hi @LuukBerkel , How did you install robot_localization from github??When i `Starting >>> robot_localization Could not find a package configuration file provided by "GeographicLib" Add the installation prefix of "GeographicLib" to CMAKE_PREFIX_PATH or set Failed <<< robot_localization [3.60s, exited with code 1] |
|
Please install install libgeographic by running |
|
I have also created ported the 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.client_futures.remove(f)
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. |
Hii @LuukBerkel , I tried doing what you suggested above ......But i am still getting the same error :
|
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!! |
|
When I do the following part of the tutorial: I get the following error: As a result nav2 doesn't seem to start (it's inactive) and when I send a goal the robot doesn't move how did you solve it? |
|
I'm trying this demo on humble too and have some error running is this errors related to humble vs iron distro? Thank |
Hii @LuukBerkel , Any updates regarding this?? |
|
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 successfullyI 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.. |
|
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 !!! |
|
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 ... |
|
I tried using the docker but found no success |
Just in case you want to give it a try again: ros_entrypoint.sh put the files on a folder and run: After it finished you can run: 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.
Hope this help! |
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!')
returnAdditionally 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_codePS: I fixed my problem with the transformation of the GPS to MAP points.. was a bad definition of the robot frame. |
|
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 : I am getting the following error : `Traceback (most recent call last): During handling of the above exception, another exception occurred: Traceback (most recent call last): |
|
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() |
|
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. |
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. |
Hello, I am having the same problem as you. Could you please explain step by step how you found the solution for Ros2 humble? |
|
…------------------ 原始邮件 ------------------
发件人: "ros-planning/navigation2_tutorials" ***@***.***>;
发送时间: 2024年3月21日(星期四) 晚上11:11
***@***.***>;
***@***.******@***.***>;
主题: Re: [ros-planning/navigation2_tutorials] nav2_gps_waypoint_follower_demo does not work on ros2 humble???! (Issue #77)
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?
—
Reply to this email directly, view it on GitHub, or unsubscribe.
You are receiving this because you commented.Message ID: ***@***.***>
|
|
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 |
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.. |
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 |
Hi @yishaiSilver .. I already fixed this. was a problem with the system definition of my robot between gps frame and the base frame. |
|
@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 |
|
Just writing that I think the spin() function from LuukBerkel incorrectly calls Otherwise the code looks great. |
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!!!!!
The text was updated successfully, but these errors were encountered: