def nav_goal_cb(msg):
global target_pose
target_pose = msg
goal = GetPathGoal(use_start_pose=False,
start_pose=msg,
target_pose=msg,
planner="planner2")
get_path_ac.send_goal(goal, done_cb=plan_done_cb)
rospy.loginfo(
"Relaying move_base_simple/goal %s from RViz to MBF's planner",
pose2d2str(msg))
def nav_goal_cb(msg):
global target_pose
target_pose = msg
goal = GetPathGoal(use_start_pose=False, start_pose=msg, target_pose=msg)
get_path_ac.send_goal(goal, done_cb=plan_done_cb)
rospy.loginfo(
"Relaying move_base_simple/goal %s from RViz to MBF's planner",
pose2d2str(msg))
rospy.sleep(1)
goal = MoveBaseGoal(target_pose=msg, controller='TEBPlanner')
move_base_ac.send_goal(goal, done_cb=move_done_cb)
def goal_cb(goal_msg):
x_goal=goal_msg.pose.position.x
y_goal=goal_msg.pose.position.y
z_goal=goal_msg.pose.orientation.z
w_goal=goal_msg.pose.orientation.w
print("x: "+str(x_goal)+" y: "+str(y_goal)+" z: "+ str(z_goal)+" w: "+str(w_goal))
client = actionlib.SimpleActionClient('/move_base_flex/get_path', GetPathAction)
client.wait_for_server()
my_goal = GetPathGoal()
my_goal.use_start_pose = False
my_goal.target_pose.header.stamp = rospy.Time.now()
my_goal.target_pose.header.frame_id = "map"
my_goal.target_pose.pose.position.x = x_goal
my_goal.target_pose.pose.position.y = y_goal
my_goal.target_pose.pose.orientation.z = z_goal
my_goal.target_pose.pose.orientation.w = w_goal
my_goal.tolerance = 0.0
my_goal.planner = "SBPLLatticePlanner"
print("sending goal")
client.send_goal(my_goal)
client.wait_for_result()
def test_server_online(self):
"""Check that server is online
In this simple setup, we don't really want to generate a plan.
Instread, we can ask for a plan on our planner (gpp_gp), and verify,
that the generated error-message is not INVALID_PLUGIN
See https://github.com/magazino/move_base_flex/blob/596ed881bfcbd847e9d296c6d38e4d3fa3b74a4d/mbf_msgs/action/GetPath.action
for reference.
"""
# setup the client
get_path = SimpleActionClient('move_base_flex/get_path', GetPathAction)
self.assertTrue(get_path.wait_for_server(rospy.Duration(60)),
"{} server offline".format(get_path.action_client.ns))
# send a dummy goal with the right planner
goal = GetPathGoal()
goal.planner = 'gpp_gp'
get_path.send_goal_and_wait(goal, rospy.Duration(1))
result = get_path.get_result()
# we are happy as long the plugin is known
self.assertNotEqual(result.outcome, GetPathResult.INVALID_PLUGIN)
def plan_done_cb(status, result):
if result.outcome == GetPathResult.SUCCESS:
rospy.loginfo("Get path action succeeded")
# result.path.poses.insert(0, result.path.poses[0]) create a fake RIP segment
# print result.path
# follow_ac.cancel_all_goals()
global target_pose
if target_pose:
goal = GetPathGoal(use_start_pose=False,
start_pose=target_pose,
target_pose=target_pose,
planner="planner2")
get_path_ac.send_goal(goal, done_cb=plan_done_cb)
target_pose = None
else:
rospy.logerr("Get path action failed with error code [%d]: %s",
result.outcome, result.message)
def on_enter(self, userdata):
# When entering this state, we send the action goal once to let the robot start its work.
# Create the goal.
goal = GetPathGoal()
goal.use_start_pose = False
goal.tolerance = self._tolerance #0.2
goal.target_pose = userdata.target_pose
goal.planner = self._planner #'global_planner'
# Send the goal.
self._error = False # make sure to reset the error state since a previous state execution might have failed
try:
self._client.send_goal(self._topic, goal)
except Exception as e:
Logger.logwarn('Failed to send goal:\n%s' % str(e))
self._error = True
if not self._error:
Logger.loginfo('Sending goal succeeded')
def nav_goal_cb(msg):
global target_pose
target_pose = msg
goal = GetPathGoal(use_start_pose=False, start_pose=msg, target_pose=msg)
get_path_ac.send_goal(goal, done_cb=plan_done_cb)