Create a global plan between any 2 poses with nav2
I am trying to create a path between an initial and a goal pose using nav2simplecommander API. I have the /map topic made available and I have a gazebo simulation of the world running.
#! /usr/bin/env python3
from rclpy.node import Node
import rclpy
from rclpy.executors import SingleThreadedExecutor
from rclpy.executors import ExternalShutdownException
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
class AgentPathPlanner(Node):
def __init__(self):
super().__init__('agent_path_planner')
self.declare_parameter('x_init', 3.0)
self.declare_parameter('y_init', 2.5)
self.declare_parameter('x_goal', 3.0)
self.declare_parameter('y_goal', 7.0)
self.setup_navigation()
def setup_navigation(self):
self.navigator = BasicNavigator()
initial_pose = PoseStamped()
goal_pose = PoseStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = self.navigator.get_clock().now().to_msg()
initial_pose.pose.position.x = self.get_parameter(
'x_init').get_parameter_value().double_value
initial_pose.pose.position.y = self.get_parameter(
'y_init').get_parameter_value().double_value
initial_pose.pose.orientation.z = 0.707
initial_pose.pose.orientation.w = 0.707
goal_pose.header.frame_id = 'map'
goal_pose.header.stamp = self.navigator.get_clock().now().to_msg()
goal_pose.pose.position.x = self.get_parameter(
'x_goal').get_parameter_value().double_value
goal_pose.pose.position.y = self.get_parameter(
'y_goal').get_parameter_value().double_value
goal_pose.pose.orientation.z = 0.707
goal_pose.pose.orientation.w = 0.707
#self.navigator.setInitialPose(initial_pose)
self.navigator.waitUntilNav2Active()
self.get_agent_path(initial_pose, goal_pose)
def get_agent_path(self, initial_pose, goal_pose):
path = self.navigator.getPath(initial_pose, goal_pose)
self.info(path)
return path
def info(self, msg):
self.get_logger().info(msg)
return
def main(args=None):
""" send the target pose to each robot in the fleet
The fleet can be scaled up and down by changing the agents to generate
more edges
"""
rclpy.init(args=args)
agent_path_planner = AgentPathPlanner()
executor = SingleThreadedExecutor()
executor.add_node(agent_path_planner)
try:
executor.spin()
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
executor.shutdown()
rclpy.try_shutdown()
if __name__ == '__main__':
main()
However when I run my Node the program waits for amcl/getstate. The initial pose I provided is neither the pose of a robot nor I have robots spawned in the simulator. I totally understand that the node waiting for the amcl/getstate is because I do not have a robot in my simulator.
[INFO] [1688985417.374920200] [basic_navigator]: amcl/get_state service not available, waiting...
[INFO] [1688985418.378249334] [basic_navigator]: amcl/get_state service not available, waiting...
[INFO] [1688985419.381707601] [basic_navigator]: amcl/get_state service not available, waiting...
But my question is Is there a way to compute a path between 2 poses in a map using nav2. What I am trying to achieve is to generate a global path from pose A to pose B without without relying on AMCL
Asked by ipa-rar on 2023-07-10 05:44:04 UTC
Comments