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 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()