def execute(self, ud): self.move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose = ud.move_waypoints[ud.move_target_wp] goal.target_pose.header.stamp = rospy.Time.now() self.move_base_client.send_goal(goal) self.move_base_client.wait_for_result() status = self.move_base_client.get_state() tts_msg = String() if status == GoalStatus.SUCCEEDED: tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "." self.tts_pub.publish(tts_msg) if ud.move_target_wp == 0: return 'test_finished' else: ud.move_target_wp -= 1 return 'succeeded' else: return 'wp_not_reached'
def _move_to_marker(self, trans_pose): pose = trans_pose goal = MoveBaseGoal() goal.target_pose = pose # The robot can't move in these directions. Clear them so it's not confused goal.target_pose.pose.position.z = 0 goal.target_pose.pose.orientation.x = 0 goal.target_pose.pose.orientation.y = 0 # If the marker position is fresh, stay behind it so we don't collide. # Use trans_pose to get original time if self._last_trans_pose.header.stamp + rospy.Duration(2) > rospy.Time.now(): goal.target_pose.pose.position.x = goal.target_pose.pose.position.x - self.FOLLOW_DIST rospy.logdebug( "Sending new goal. x: " + str(goal.target_pose.pose.position.x) + ", y: " + str(goal.target_pose.pose.position.y) ) # Cancel old goal if it exists. self.base_client.cancel_all_goals() self.base_client.send_goal(goal)
def navigate(self, goalHandle, statusCB, doneCB): positions = goalHandle.get_goal().jointPositions pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "map" pose.pose.position.x = positions[0] pose.pose.position.y = positions[1] pose.pose.position.z = 0.0 q = Rotation.RotZ(positions[2]).GetQuaternion() pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] client_goal = MoveBaseGoal() client_goal.target_pose = pose client = self._jointControllers.get('base', None) if client: client.wait_for_server() rospy.loginfo("%s: Navigating to %s", rospy.get_name(), pose) client.send_goal(client_goal, done_cb=doneCB) else: goalHandle.set_rejected()
def go_to(self, goal_posestamped, timeout=0): goal = MoveBaseGoal() goal.target_pose = goal_posestamped self.movebase.send_goal(goal) self.movebase.wait_for_result() # timeout maybe? # result = self.movebase.get_goal_status_text() # print 'move_base ' + result resultcode = self.movebase.get_state() # print 'result code ' + repr(resultcode) return resultcode
def move_to(self, stamped, done_cb, timeout_cb): goal = MoveBaseGoal() goal.target_pose = stamped goal.target_pose.header.stamp = rospy.Time(0) msg = MarkerArray() msg.markers.append(self.tools.create_pose_marker(stamped)) self.tools.visualization_publisher.publish(msg) self.cancel_all_actions() self.cancel_standstill_timer() self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().move_base_timeout, action_client=self.move_base_client, goal=goal)
def go_to(self, pose_stamped): """Goes to a location in the world. Args: pose_stamped: geometry_msgs/PoseStamped. The location to go to. """ self._move_base_client.wait_for_server() goal = MoveBaseGoal() goal.target_pose = pose_stamped self._move_base_client.send_goal(goal) self._move_base_client.wait_for_result()
def goalcallback(self, data): #we'll send a goal to the robot to move 3 meters forward goal = MoveBaseGoal() # goal.target_pose.header.frame_id = 'base_link' # goal.target_pose.header.stamp = rospy.Time.now() # goal.target_pose.pose.position.x = 0.2 # goal.target_pose.pose.orientation.w = 1.0 goal.target_pose = data #start moving rospy.logerr(goal) # self.move_base.send_goal(goal) #TODO check status of route, if on another route, cancel it first # self.cancelMovement() t = rospy.Time.now() #draws the path req = GetPlanRequest() req.start = self.robot_pose_stamped req.start.header.stamp = t req.goal = data req.goal.header.stamp = t req.tolerance = .02 # return_path = self.get_path_to_goal_srv(req) # self.next_path_pub.publish(return_path) self.move_to_goal_count += 1 g = MoveBaseActionGoal(); t = rospy.Time.now() g.header = Header(self.move_to_goal_count, t, "map") g.goal_id.stamp = t g.goal_id.id = "movement_num:"+str(self.move_to_goal_count) g.goal.target_pose = data # newnppose = vector3_to_numpy(data.pose.position) # if np.linalg.norm(newnppose - self.lastnppose) >.05 : # self.goal_pose_stamped_pub.publish(data) self.move_to_goal_pub.publish(g) rospy.logerr("published move_base goal")
def retreat(self, done_cb, timeout_cb): goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'base_footprint' goal.target_pose.pose.position.x = -(Params().approach_distance - Params().ring_distance) goal.target_pose.header.stamp = rospy.Time(0) goal.target_pose.pose.orientation.w = 1 goal.target_pose = self.tools.transform_pose('map', goal.target_pose) msg = MarkerArray() msg.markers.append(self.tools.create_pose_marker(goal.target_pose)) msg.markers[-1].color.r = 0.8 msg.markers[-1].color.g = 0.5 self.tools.visualization_publisher.publish(msg) self.cancel_all_actions() self.cancel_standstill_timer() self._action = ActionWrapper(done_cb=done_cb, timeout_cb=timeout_cb, timeout=Params().approach_timeout, action_client=self.retreat_client, goal=goal)
def move_to_pose(self, pos, frame="/odom_combined"): """ Move the robot to pos, using the move_base actions. @param pos: A PoseStamped @param frame: Check that the frame is allowed by move_base """ if not self.use_move_base: rospy.logerr("Trying to call a move_base service without \ initializing it") self.listener.waitForTransform(frame, pos.header.frame_id, rospy.Time(0), rospy.Duration((1.0))) target = self.listener.transformPose(frame, pos) goal= MoveBaseGoal() goal.target_pose = target self.move_action.send_goal(goal) return self.move_action.wait_for_result()
def driveTo(x,y, theta): rospy.loginfo("Drive to (%f, %f, %f)", x, y, theta) newPose = generate_pose(x, y, 0) #print newPose actionGoal = MoveBaseActionGoal() actionGoal.header = genHeader() actionGoal.goal_id.id = str(driveTo.goalID) actionGoal.goal_id.stamp = rospy.Time.now() goal = MoveBaseGoal() goal.target_pose = newPose actionGoal.goal = goal # Publish the goal to the robot global actionGoalPublisher actionGoalPublisher.publish(actionGoal) global soundClient # Wait for the robot's status to to have reached the goal timeOutCounter = 0 while not rospy.is_shutdown(): # This is done so that status can be checked and used rospy.sleep(4.) timeOutCounter += 1 currentStatus = getStatus(driveTo.goalID) global cant_reach_list print "Status: %d, GoalID: %d, Driving to: (%f, %f, %f), # unreachable: %d" % (currentStatus, driveTo.goalID, x, y, theta, len(cant_reach_list )) if currentStatus == GoalStatus.ABORTED or timeOutCounter > 20: soundClient.say("Abort driving to goal.") print "The goal was aborted" cant_reach_list.append((x, y)) break elif currentStatus == GoalStatus.REJECTED: soundClient.say("Goal rejected") print "The goal was rejected" break elif currentStatus == GoalStatus.LOST: soundClient.say("Robot is lost") print "The robot is lost, exiting driving" #TODO Should we send a cancel message? exit(1) break elif currentStatus == GoalStatus.SUCCEEDED: soundClient.say("Goal reached. Moving on.") print "Drive to complete!" break driveTo.goalID += 1
def execute(self, userdata): rospy.loginfo("MoveBase called.") try: client = actionlib.SimpleActionClient('move_base', MoveBaseAction) client.wait_for_server(rospy.Duration.from_sec(10.0)) goal = MoveBaseGoal() goal.target_pose = userdata.goal_pose client.send_goal(goal) client.wait_for_result(rospy.Duration.from_sec(80.0)) if client.get_state() == 3: return 'succeeded' else: return 'failed' except actionlib.ActionException, e: rospy.logwarn("MoveBase action failed: %s" %e) return 'failed'
def move(self, goal_x=0, goal_y=0, goal_yaw=0, x_lower_bound=-1, x_upper_bound=1, y_lower_bound=-1, y_upper_bound=1): # To prevent robot getting stuck or drifting towards just one specific area over course of experiment # Randomly send to middle of allowable area with 1% probability rand = random.uniform(0, 100) if rand == 1: goal_x = (x_lower_bound + x_upper_bound) / 2 goal_y = (y_lower_bound + y_upper_bound) / 2 goal_pose = PoseStamped() goal_pose.header.stamp = rospy.Time.now() goal_pose.header.frame_id = 'map' goal_pose.pose.position.x = goal_x goal_pose.pose.position.y = goal_y goal_pose.pose.orientation = self.continuous_pose_wrt_map.pose.pose.orientation # Always keep our orientation the same goal = MoveBaseGoal() goal.target_pose = goal_pose try: rospy.logdebug('Waiting for move_base server') self.move_base_client.wait_for_server() rospy.logdebug('Sending goal to move_base server') self.move_base_client.send_goal(goal) rospy.logdebug('Waiting for goal result from move_base server') self.move_base_client.wait_for_result() success = self.move_base_client.get_result() if not success: rospy.logwarn(self.namespace + ': move_base was not able to successfully complete the request action') else: continuous_x = self.continuous_pose_wrt_map.pose.pose.position.x continuous_y = self.continuous_pose_wrt_map.pose.pose.position.y discrete_x = self.discrete_pose_wrt_map.pose.pose.position.x discrete_y = self.discrete_pose_wrt_map.pose.pose.position.y gazebo_x = self.gazebo_pose_wrt_map.pose.pose.position.x gazebo_y = self.gazebo_pose_wrt_map.pose.pose.position.y rospy.loginfo(self.namespace + ': requested move to (' + str(goal_x) + ', ' + str(goal_y) + ') ' + 'completed. Current gazebo odom pose is (' + str(gazebo_x) + ', ' + str(gazebo_y) + '). ' + 'Current continuous odom pose is (' + str(continuous_x) + ', ' + str(continuous_y) + '). ' + 'Current discrete odom pose is (' + str(discrete_x) + ', ' + str(discrete_y) + '). ') except rospy.ROSException as e: rospy.logwarn(self.namespace + ': caught exception while sending move_base a movement goal - ' + e.message)
def set_goal(self, goal_id): if self.current_goal_status() != "FREE": self.cancel_goal() # invalid goal id if goal_id >= len(self.waypoints): self.goal_status = "ERROR" rospy.logerr("Invalid goal id: " + str(goal_id)) return self.current_goal_id = goal_id self.goal_status = "WORKING" goal = MoveBaseGoal() goal.target_pose = self.waypoints[goal_id] def done_cb(status, result): self.goal_status = "FREE" # wait for 1s if not self.move_base.wait_for_server(rospy.Duration(1)): self.goal_status = "ERROR" self.cancel_goal() return self.move_base.send_goal(goal, done_cb=done_cb)
def begin_explore( self, radius, preempt_func = retfalse ): rospy.logout( 'snaking_explore: setting radius' ) waypoints = self.setup_poses( radius ) # PoseStamped in self.frame local_planner = rospy.get_param('/move_base_node/base_local_planner') # : dwa_local_planner/DWAPlannerROS local_planner = local_planner[string.find(local_planner,'/')+1:] obs_range = rospy.get_param('/move_base_node/global_costmap/obstacle_range') move_tol = rospy.get_param('/move_base_node/'+local_planner+'/xy_goal_tolerance') no_progress = rospy.get_param( rospy.get_name() + '/no_progress_move', 5) unreached_time = rospy.get_param( rospy.get_name() + '/not_reached_move', 30) # I'm not entirely sure which service to use. I do know that # non-NavfnROS make_plan sometimes fails for an unknown # reason... and thus NavfnROS/make_plan is more robust. # srv_name = '/move_base_node/make_plan' srv_name = '/move_base_node/NavfnROS/make_plan' get_plan = rospy.ServiceProxy( srv_name, GetPlan ) # Clear costmap...? Do this here or in smach...? if preempt_func(): # immediately exit if overall action preempted return 'preempted' for i,w in enumerate( waypoints ): if preempt_func(): # immediately exit if overall action preempted return 'preempted' rospy.logout( 'snaking_explore: Seeking waypoint %d of %d' % (i+1,len(waypoints))) # rospy.logout( 'snaking_explore: %2.2f %2.2f in frame %s' % (w.pose.position.x, w.pose.position.y, w.header.frame_id)) rim = self.robot_in_map() w.header.stamp = rospy.Time(0) # transform it _now_ wim = self.ps_in_map( w ) # waypoint in map if not rim or not wim: # if transforms failed rospy.logout( 'snaking_explore: Bad transforms. Aborting explore' ) return 'aborted' if self.range_to_waypoint( rim, wim ) < 0.9 * obs_range: # We're nearby the waypoint, so we'll just trust that the planner # has a good view of its surroundings to determine reachability. req = GetPlanRequest() req.tolerance = 0.1 req.start = rim req.goal = wim resp = get_plan( req ) found_plan = bool( resp.plan.poses != [] ) if not found_plan: rospy.logout( 'snaking_explore: No plan to nearby waypoint. Proceeding to next waypoint' ) # Perhaps its worth pulling the waypoint closer until a path _is_ found? continue # We now know that a path exists. Send the waypoint to the client. rospy.logout( 'snaking_explore: Near by with good plan. Calling move_base action.' ) else: # Any nav plan at beyond obstacle range will require the # nav stack to get a better vantage. Send the waypoint to # the client. rospy.logout( 'snaking_explore: Far away. Calling move_base action.' ) # If we made it this far, it's time to call move_base action. # Cancel any previous goals self.client.cancel_goals_at_and_before_time( rospy.Time.now() ) if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim': rospy.logout( 'running in sim: give the system a little time to respond' ) rospy.sleep( 1 ) # Send our new goal rospy.logout( 'snaking_explore: sending movebase action goal.' ) move_goal = MoveBaseGoal() move_goal.target_pose = wim self.client.send_goal( move_goal ) if os.environ.has_key('ROBOT') and os.environ['ROBOT'] == 'sim': rospy.logout( 'running in sim: give the system a little time to respond' ) rospy.sleep( 1 ) # We'll monitor the state ourselves, and allow ourselves # the opportunity to cancel the goal for various reasons # (eg. if we haven't moved for a while or if new plans # aren't found after getting within obstacle distance) # When this loop is broken, we'll go to the next goal. We # don't need to cancel the goals here -- they will be # cancelled before a new one is sent. r = rospy.Rate( 10 ) xytt_old = None # Checks for movement stime = rospy.Time.now().to_sec() while True: if self.client.simple_state == SimpleGoalState.DONE: res = self.client.get_result() rospy.logout('snaking_explore: Movebase actionlib completed with result. Proceeding to next waypoint') break rim = self.robot_in_map() w.header.stamp = rospy.Time( 0 ) wim = self.ps_in_map( w ) # Proceed when close enough to goal (within two tolerance factors) if rim and wim: # only do this check if transform succeeds if self.range_to_waypoint( rim, wim ) < move_tol * 2.0: rospy.logout( 'snaking_explore: Close enough to waypoint. Proceeding to next waypoint' ) break # We're near the waypoint, so let's must move on to next # Proceed when there has been no movement (x,y, or theta) for X sec. if rim: # only do this check if transform succeeds xytt = self.xythetatime( rim ) if not xytt_old: xytt_old = xytt if np.abs( xytt[0] - xytt_old[0] ) > 0.05 or \ np.abs( xytt[1] - xytt_old[1] ) > 0.05 or \ np.abs( xytt[2] - xytt_old[2] ) > math.radians(10): xytt_old = xytt if xytt[3] - xytt_old[3] > no_progress: rospy.logout( 'snaking_explore: No movement in %2.1f seconds. Proceeding to next waypoint' % no_progress ) break if rospy.Time.now().to_sec() - stime > unreached_time: rospy.logout( 'snaking_explore: Goal unreached in %2.1f seconds. Proceeding to next waypoint' % unreached_time ) break r.sleep() self.client.cancel_all_goals() rospy.logout( 'snaking_explore: Finished with exploration.' ) return 'succeeded'
def execute(self, target, blocking=True): component_name = "base" ah = ActionHandle("move_base", component_name, target, blocking) rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target) #get pose from parameter server if type(target) is str: if not rospy.has_param(self.ns_global_prefix + "/" + component_name + "/" + target): rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.ns_global_prefix + "/" + component_name + "/" + target) ah.set_failed(2) return ah param = rospy.get_param(self.ns_global_prefix + "/" + component_name + "/" + target) else: param = target #check pose if not type(param) is list: # check outer list rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name) #print "parameter is:", param ah.set_failed(3) return ah else: #print i,"type1 = ", type(i) DOF = 3 if not len(param) == DOF: # check dimension rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param)) #print "parameter is:", param ah.set_failed(3) return ah else: for i in param: #print i,"type2 = ", type(i) if not ((type(i) is float) or (type(i) is int)): #check type #print type(i) rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name) print "parameter is:", param ah.set_failed(3) return ah else: rospy.logdebug("accepted parameter %f for %s", i, component_name) #convert to pose message pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "/map" pose.pose.position.x = param[0] pose.pose.position.y = param[1] pose.pose.position.z = 0.0 q = tf.transformations.quaternion_from_euler(0, 0, param[2]) pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] action_server_name = "/move_base" rospy.logdebug("calling %s action server", action_server_name) client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction) #trying to connect to server rospy.logdebug("waiting for %s action server to start", action_server_name) if not client.wait_for_server(rospy.Duration(5)): #error: server did not respond rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name) ah.set_failed(4) return ah else: rospy.logdebug("%s action server ready", action_server_name) # sending goal client_goal = MoveBaseGoal() client_goal.target_pose = pose print client_goal client.send_goal(client_goal) ah.set_client(client) ah.wait_inside() return ah
#!/usr/bin/env python import rospy from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from actionlib import SimpleActionClient from geometry_msgs.msg import PoseStamped if __name__ == '__main__': rospy.init_node('robocup_test_node') goal = MoveBaseGoal() pose = PoseStamped() pose.header.frame_id = 'map' pose.header.stamp = rospy.Time.now() pose.pose.position.x = 1.0 pose.pose.position.y = 1.0 pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.z = 1.0 goal.target_pose = pose move_base_client = SimpleActionClient('move_base', MoveBaseAction) move_base_client.send_goal(goal) move_base_client.wait_for_result(rospy.Duration(20))
if len(goal_list) == 0: all_bad_cnt += 1 if all_bad_cnt > 2: sounds.publish(Sound(0)) rospy.sleep(2.) sounds.publish(Sound(1)) rospy.sleep(2.) sys.exit() break goal = goal_list.pop(0)[1] test_plan = do_astar(goal, pose, 0.0).plan.poses print test_plan goal_lock = False dist = pose_dist(pose, goal) print dist #path = do_astar(pose, goal, 0.0).plan go_pub.publish(goal) pub_goal = MoveBaseGoal() pub_goal.target_pose = goal go_action.send_goal(pub_goal) result = go_action.wait_for_result() goal_blacklist.append(goal) goal_list.pop(0) if go_action.get_state() == 4: do_circle = False else: do_circle = True except Exception as exc: print exc
def person_position_goal_cb(userdata, nav_goal): move_base_goal = MoveBaseGoal() move_base_goal.target_pose = userdata.person_pose move_base_goal.target_pose.header.stamp = rospy.Time.now() return move_base_goal
def main(): rospy.init_node('smach_example_state_machine') # Create the top level SMACH state machine sm_top = smach.StateMachine(outcomes=['done', 'exit']) # Open the container with sm_top: goal1=MoveBaseGoal() goal1.target_pose.header.frame_id = "dtw_robot1/map" goal1.target_pose.pose.position.x = 2.7 goal1.target_pose.pose.position.y = 2.8 goal1.target_pose.pose.orientation.w = 1.0 smach.StateMachine.add('MOVE1', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal1), transitions={'succeeded':'MOVE2', 'preempted':'MOVE1', 'aborted':'MOVE1'}) goal2=MoveBaseGoal() goal2.target_pose.header.frame_id = "dtw_robot1/map" goal2.target_pose.pose.position.x = 4.3 goal2.target_pose.pose.position.y = 3.8 goal2.target_pose.pose.orientation.w = 1.0 smach.StateMachine.add('MOVE2', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal2), transitions={'succeeded':'TASK2', 'preempted':'exit', 'aborted':'TASK2'}) sm_sub = smach.StateMachine(outcomes=['done']) with sm_sub: #vel_pub = rospy.Publisher('/dtw_robot1/diff_drive_controller/cmd_vel', Twist, queue_size=10) #vel = Twist() #callback_lambda = lambda x: callback(x, vel, vel_pub) #goal=MoveBaseGoal() #goal.target_pose.header.frame_id = "dtw_robot1/map" #goal.target_pose.pose.position.x = 4.5 #goal.target_pose.pose.orientation.w = 1.0 #smach.Concurrence.add('MOVE', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal)) #vel_pub = rospy.Publisher('/dtw_robot1/diff_drive_controller/cmd_vel', Twist, queue_size=10) #vel = Twist() #callback_lambda = lambda x: callback(x, vel, vel_pub) #while not rospy.is_shutdown(): # rospy.init_node('listener', anonymous=True) # rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes , callback_lambda) # rospy.spin() #smach.Concurrence.add('DETECTION', smach_ros.DetectionState("/dtw_robot1/diff_drive_controller/cmd_vel", Twist, callback)) smach.StateMachine.add('STATE1', State1(), transitions={'done':'STATE2', 'exit':'done'}) smach.StateMachine.add('STATE2', State2(), transitions={'done':'STATE1'}) smach.StateMachine.add('TASK2', sm_sub, transitions={'done':'MOVE3'}) goal3=MoveBaseGoal() goal3.target_pose.header.frame_id = "dtw_robot1/map" goal3.target_pose.pose.position.x = 4.5 goal3.target_pose.pose.position.y = 1.7 goal3.target_pose.pose.orientation.w = 1.0 smach.StateMachine.add('MOVE3', smach_ros.SimpleActionState('/dtw_robot1/move_base', MoveBaseAction, goal=goal3), transitions={'succeeded':'exit', 'preempted':'MOVE1', 'aborted':'MOVE2'}) # Execute SMACH plan sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT') sis.start() outcome = sm_top.execute() rospy.spin() sis.stop()