def send_move_base_goal(posx, posy, rotationz): global last_goal_id ag = MoveBaseActionGoal() ag.header = Header() ag.goal_id = GoalID() ID = "0" if(not last_goal_id == None): print "Last goal id:" print last_goal_id IDint = int(float(last_goal_id.id))+1 ID = str(IDint) print "ID:" print ID print "\n" #ag.header.seq = ID ag.goal_id.id = ID print ag.goal_id ag.goal = MoveBaseGoal() ag.goal.target_pose.header.frame_id = "map" ag.goal.target_pose.pose.position.x = posx ag.goal.target_pose.pose.position.y = posy dummy = tf.transformations.quaternion_from_euler(0, 0, rotationz) ag.goal.target_pose.pose.orientation.x = dummy[0] ag.goal.target_pose.pose.orientation.y = dummy[1] ag.goal.target_pose.pose.orientation.z = dummy[2] ag.goal.target_pose.pose.orientation.w = dummy[3] print ag pub_goal.publish(ag) last_goal_id = ag.goal_id pub_log.publish(rospy.get_name()+": published ActionGoal")
def publishNextPoint(self): x_next, y_next = self.convert_node_to_point(self.next_node) w_next = self.computeNextOrientation(x_next, y_next) rospy.loginfo( "[%s] Converted next node to point: (%s,%s), orientation: %s" % (self.node_name, x_next, y_next, w_next)) goal_id = GoalID() goal_id.id = str(self.goal_idx) msg = MoveBaseActionGoal() msg.goal_id = goal_id msg.goal.target_pose.header.frame_id = self.frame_id msg.goal.target_pose.pose.position.x = x_next msg.goal.target_pose.pose.position.y = y_next msg.goal.target_pose.pose.position.z = 0 msg.goal.target_pose.pose.orientation.x = 0 msg.goal.target_pose.pose.orientation.y = 0 msg.goal.target_pose.pose.orientation.z = math.sin(w_next / 2) msg.goal.target_pose.pose.orientation.w = math.cos(w_next / 2) self.pub_goal.publish(msg) rospy.loginfo("[%s] Dispatched goal point: (%s,%s)" % (self.node_name, x_next, y_next)) if self.next_node == self.goal: self.dispatched_end = True
def get_goal_to_publish(): # Get a random goal and go there # We could try to improve this random search, and have some memory global latest_goal_time global latest_goal_id global goal_points_list if latest_goal_id == None: latest_goal_id = 1 latest_goal_time = time.time() else: if time.time() - latest_goal_time > 20.: latest_goal_id += 1 latest_goal_time = time.time() else: # Print should not happen anymore print "Too little time passed since last goal sent" return None goal_to_send = MoveBaseActionGoal() current_goal_id = GoalID() current_goal_id.id = str(latest_goal_id) goal_to_send.goal_id = current_goal_id pose_stamped = PoseStamped() pose = Pose() idx = random.randrange(len(goal_points_list)) pose.position.x = goal_points_list[idx][0] pose.position.y = goal_points_list[idx][1] roll = 0. pitch = 0. yaw = 0. quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] pose_stamped.pose = pose goal = MoveBaseGoal() goal.target_pose = pose_stamped goal.target_pose.header.frame_id = 'map' goal_to_send.goal = goal return goal_to_send
def go_to_location(self, location, frame_id, timeout=50.0): ''' description: move the robot to the desired location input: string location, the name of the location that you want the robot to go double timeout, max time to allow to reach the location before returning string frame_id, frame where the pose is located NOTE: the location needs to be loaded in the parameter server in advance, to get a list of availbale locations use mbot.get_available_locations() function output: bool success, whether if the robot was succesful or not at reaching the goal ''' timeout_ros = rospy.Duration(timeout) time_start = rospy.Time.now() try: location_pose = self.get_available_poses()[location] except KeyError: rospy.logerr( '{} is not a valid waypoint from this list: {}'.format( location, self.get_available_poses())) return None goal = PoseStamped() if not isinstance(frame_id, str): frame_id = 'odom' goal.header.frame_id = frame_id goal.header.stamp = rospy.Time.now() quaternion = tf.transformations.quaternion_from_euler( 0, 0, location_pose[2]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] goal.pose.position.x = location_pose[0] goal.pose.position.y = location_pose[1] goal.pose.position.z = 0 final_goal = MoveBaseActionGoal() final_goal.header = goal.header final_goal.goal_id.stamp = rospy.Time.now() final_goal.goal_id.id = str(random()) goal_id = final_goal.goal_id final_goal.goal.target_pose = goal self.__move_base_safe_pub.publish(final_goal) rospy.loginfo("Sending goal to move_base_simple, destination : " + location) while not rospy.is_shutdown( ) and rospy.Time.now() - time_start < timeout_ros: if self.__goals_status: for i in self.__goals_status: if i.goal_id == goal_id and i.status == 3: return True return False
def set_destination(self, destination): print destination['latitude'] print destination['longitude'] msg = MoveBaseActionGoal() msg.header.stamp = rospy.get_rostime() msg.header.frame_id = frame_id msg.sentence = sentence self.pub.publish(msg) return "a"
def get_action(self): mba = MoveBaseAction() ag_ = MoveBaseActionGoal() ag_.header = Header() ag_.header.frame_id = "map" ag_.header.stamp = rospy.Time.now() ag_.goal = MoveBaseGoal() ag_.goal.target_pose = self.target_waypoint mba.action_goal = ag_ return mba
def go_to_pose(self, location_pose, frame_id, back_up=[1, 1], timeout=50): ''' description: move the robot to the desired pose needs: mir_move_base_safe and mbot_actions move_base_server input: PoseStamped location_pose, x, y, yaw double timeout, max time to allow to reach the location before returning string frame_id, frame where the pose is located output: bool success, whether if the robot was succesful or not at reaching the goal ''' timeout_ros = rospy.Duration(timeout) time_start = rospy.Time.now() goal = PoseStamped() if not isinstance(frame_id, str): frame_id = 'odom' goal.header.frame_id = frame_id goal.header.stamp = rospy.Time.now() quaternion = tf.transformations.quaternion_from_euler( 0, 0, location_pose[2]) goal.pose.orientation.x = quaternion[0] goal.pose.orientation.y = quaternion[1] goal.pose.orientation.z = quaternion[2] goal.pose.orientation.w = quaternion[3] goal.pose.position.x = location_pose[0] goal.pose.position.y = location_pose[1] goal.pose.position.z = 0 final_goal = MoveBaseActionGoal() final_goal.header = goal.header final_goal.goal_id.stamp = rospy.Time.now() final_goal.goal_id.id = str(random()) goal_id = final_goal.goal_id final_goal.goal.target_pose = goal self.__move_base_safe_pub.publish(final_goal) rospy.loginfo("Sending goal to move_base_simple, destination : (" + str(location_pose[0]) + "," + str(location_pose[1]) + ") with yaw: " + str(location_pose[2])) while not rospy.is_shutdown( ) and rospy.Time.now() - time_start < timeout_ros: if self.__goals_status: for i in self.__goals_status: if i.goal_id == goal_id and i.status == 3: return True if i.goal_id == goal_id and i.status == 4: return self.go_to_pose([ location_pose[0] + back_up[0], location_pose[1] + back_up[1], location_pose[2] ], frame_id=frame_id, back_up=back_up, timeout=timeout) return False
def generate_goal(): global goal pose_stamped = PoseStamped() pose_stamped.pose = None # no goal at start move_base_goal = MoveBaseGoal() move_base_goal.target_pose = pose_stamped goal = MoveBaseActionGoal() goal.goal = move_base_goal
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 __init__(self): self.cob_marker_goal = DetectObjectsGoal() self.MovePTP_goal = MoveLinGoal() self.MoveStart_goal = MoveLinGoal() self.MoveBaseHome_goal = MoveBaseActionGoal() self.MoveBaseShelf_goal = MoveBaseActionGoal() # protected region initCode on begin # genpy.message.fill_message_args(self.MovePTP_goal, [rospy.get_param('/simple_pnp/MovePTPGoal')]) genpy.message.fill_message_args(self.MoveStart_goal, [rospy.get_param('/simple_pnp/MoveStartGoal')]) genpy.message.fill_message_args(self.MoveBaseHome_goal, [rospy.get_param('/simple_pnp/MoveBaseHomeGoal')]) genpy.message.fill_message_args(self.MoveBaseShelf_goal, [rospy.get_param('/simple_pnp/MoveBaseShelfGoal')]) genpy.message.fill_message_args(self.cob_marker_goal, [rospy.get_param('/simple_pnp/CobMarkerGoal')]) # protected region initCode end # pass
def __init__(self): self.goal_id = 0 print("reached?") self.marker = Marker() self.marker.type = Marker.SPHERE self.marker.action = Marker.ADD self.marker.scale.x = 10 self.marker.scale.y = 10 self.marker.scale.z = 10 self.marker.color.a = 1 self.marker.pose.orientation.w = 1.0 self.marker.pose.position.x = 2 self.marker.pose.position.y = 2 self.marker.pose.position.z = 2 self.marker.header.frame_id = '/map' self.goal_message = MoveBaseActionGoal() rospy.init_node("goal_generator", anonymous=False) rospy.loginfo("Goal Generator initiated") # rospy.on_shutdown(self.shutdown) rospy.Subscriber("/move_base/result", MoveBaseActionResult, self.goalCallback) self.rbt_goal = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=2) self.rbt_goal.publish(self.goal_message) self.vis_goal = rospy.Publisher('/visualization_marker', Marker, queue_size=2) self.vis_goal.publish(self.marker) rospy.loginfo(self.marker) rospy.spin()
def set_goal(x,y): ac_goal = MoveBaseActionGoal() ac_goal.goal.target_pose.header.frame_id = "map" ac_goal.goal.target_pose.pose.orientation.w = 1.0 ac_goal.goal.target_pose.pose.position.x = x ac_goal.goal.target_pose.pose.position.y = y pub2.publish(ac_goal)
def __init__(self): rospy.init_node('MultiGoalListen', anonymous=False) rospy.on_shutdown(self.shutdown) self.r = redis.Redis(host="127.0.0.1", port=6379, db=0) if os.path.exists("/home/eaibot/dashgo_ws/src/dashgo/dashgo_nav/maps/current_pose.json")==False: open("/home/eaibot/dashgo_ws/src/dashgo/dashgo_nav/maps/current_pose.json","w") rospy.Subscriber('/robot_pose', Pose, self.robot_pose_callback) self.r.set("RosServiceState", 1) self.goal_pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=5) self.goal_cancel = rospy.Publisher('/move_base/cancel', GoalID, queue_size=5) rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.status_callback) rospy.Subscriber('/multi_cancel_goal', Int16, self.cancel_callback) rospy.Subscriber('/multi_publish_goal', Int16, self.publish_callback) attr_dict = { "mode": "loop", "loopWay": "auto", "isNavNext": 0, "currentState": "stopped", "goalQueue": "GoalQueueA", "currentGoal": "", "successNum": "0", "failedNum": "0", "intervalTime": "3", } self.r.hmset("GoalState", attr_dict) self.goal = MoveBaseActionGoal() self.isContinue=False self.isNavStop=False self.lastGoalName="" self.failRun=True rospy.spin()
def husky_mover(): pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=0) rospy.init_node('husky_mover') myGoal = MoveBaseActionGoal() targetPos = PoseStamped() rospy.sleep(1) rospy.loginfo("Setting the robot goal.") myGoal.goal.target_pose.header.frame_id = "odom" myGoal.goal.target_pose.header.stamp = rospy.Time.now() myGoal.goal.target_pose.pose.position.x = 9 myGoal.goal.target_pose.pose.position.y = 3 q = tf.transformations.quaternion_from_euler(0, 0, math.pi) myGoal.goal.target_pose.pose.orientation.x = q[0] myGoal.goal.target_pose.pose.orientation.y = q[1] myGoal.goal.target_pose.pose.orientation.z = q[2] myGoal.goal.target_pose.pose.orientation.w = q[3] pub.publish(myGoal) rospy.loginfo("Node exiting.") rospy.sleep(30); rospy.loginfo("Setting the robot goal.") myGoal.goal.target_pose.header.frame_id = "odom"; myGoal.goal.target_pose.header.stamp = rospy.Time.now(); myGoal.goal.target_pose.pose.position.x = 1; pub.publish(myGoal) rospy.loginfo("Node exiting.")
def __init__(self, area, sc, task_id): self.area = area self.sc = sc self.task_id = task_id self.CM0 = Marker2D("AreaScanMarker/obstacle_marker", 1.0, 0.0, 0.0, task_id=self.task_id) self.CM1 = Marker2D("AreaScanMarker/area_marker", 0.0, 0.0, 1.0, marker_type=4, task_id=self.task_id) self.CM2 = Marker2D("AreaScanMarker/vec_marker", 0.0, 1.0, 0.0, marker_type=0, task_id=self.task_id) self.x_list = np.zeros(0) self.y_list = np.zeros(0) # pos = [position.x, position.y, angle] self.pos = np.array([0, 0, 0], dtype='float') self.angle_list = np.zeros(0) self.listener = tf.TransformListener() self.trans = 0 self.rot = 0 self.status = GoalStatusArray() self.goal = MoveBaseActionGoal() self.map = OccupancyGrid() self.reshaped_map = np.zeros(0, dtype='int')
def add_waypoint(self, goal_with_priority, add_after_current = False, need_tf = False): # GoalWithPriority goal_id = self.new_goal_id() pose = goal_with_priority.pose goal_with_priority.goal_id = goal_id move_base_msg = MoveBaseActionGoal() move_base_msg.header.frame_id = 'odom' move_base_msg.goal.target_pose.header.frame_id = 'odom' move_base_msg.goal.target_pose.pose = pose move_base_msg.goal_id = goal_id goal = GoalManager.GoalWrapper(move_base_msg, goal_with_priority, need_tf = need_tf) goal.goal_with_priority = goal_with_priority if add_after_current: self.goals.insert(self.current_idx + 1, goal) else: self.goals.append(goal) rospy.loginfo("Received goal:\n\tx:%f\n\ty:%f" % (pose.position.x, pose.position.y))
def move(): # Starts a new node rospy.init_node('plat_move', anonymous=True) velocity_publisher = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=10) cmd = MoveBaseActionGoal() #rate = rospy.Rate(0.0636) # 0.1hz #Receiveing the user's input print("Let's move your marslite lol") while not rospy.is_shutdown(): rospy.sleep(2.) raw_input("Press Enter to continue...") print("outside button") cmd.goal.target_pose.header.frame_id = 'map' cmd.goal.target_pose.pose.position.x = 1.054 cmd.goal.target_pose.pose.position.y = -0.045 cmd.goal.target_pose.pose.orientation.z = 0.917 cmd.goal.target_pose.pose.orientation.w = -0.398 velocity_publisher.publish(cmd) rospy.sleep(2.) raw_input("Press Enter to continue...") print("outside door") cmd.goal.target_pose.header.frame_id = 'map' cmd.goal.target_pose.pose.position.x = 1.075 cmd.goal.target_pose.pose.position.y = 0.199 cmd.goal.target_pose.pose.orientation.z = -0.368 cmd.goal.target_pose.pose.orientation.w = 0.930 velocity_publisher.publish(cmd)
def set_new_pos(): global srv_client_user_interface_assignment_, previous_state_ #ask to user to insert the input print( "Insert a new position between (-4,-3) (-4,2) (-4,7) (5,-7) (5,-3) (5,1)" ) x = float(raw_input('x :')) y = float(raw_input('y :')) #check the input if ((x == -4 and (y == 2 or y == -3 or y == 7)) or (x == 5 and (y == 1 or y == -3 or y == -7))): #if input is valid #set the input values in parameters for desired position rospy.set_param("des_pos_x", x) rospy.set_param("des_pos_y", y) #public a message MovebaseActionGoal() in topic /move_base/goal msg_goal = MoveBaseActionGoal() msg_goal.goal.target_pose.header.frame_id = "map" msg_goal.goal.target_pose.pose.orientation.w = 1 #set in the position the desired ones msg_goal.goal.target_pose.pose.position.x = rospy.get_param( 'des_pos_x') msg_goal.goal.target_pose.pose.position.y = rospy.get_param( 'des_pos_y') pub2.publish(msg_goal) print("goal: x=" + str(rospy.get_param('des_pos_x')) + " y=" + str(rospy.get_param('des_pos_y'))) resp = srv_client_user_interface_assignment_() else: #if input is not valid tell it to user print("NOT VALID INPUT") print('please insert another position') check_status() return []
def compute_initial_pose(self): self.initial_pose = rospy.wait_for_message('/pose', PoseStamped) rospy.loginfo("The default position is: {}".format(self.initial_pose)) # Trasfrom from PoseStamped to MoveBaseActionGoal. move_base = MoveBaseGoal(self.initial_pose) move_base_action = MoveBaseActionGoal() move_base_action.goal = move_base move_base_action.goal.target_pose.header.frame_id = "map" move_base_action.header.stamp.secs = self.initial_pose.header.stamp.secs move_base_action.header.stamp.nsecs = self.initial_pose.header.stamp.nsecs move_base_action.header.frame_id = "map" self.goal = move_base_action
def on_enter(self, userdata): # This method is called when the state becomes active, i.e. a transition from another state to this one is taken. # It is primarily used to start actions which are associated with this state. # The following code is just for illustrating how the behavior logger works. # Text logged by the behavior logger is sent to the operator and displayed in the GUI. ##time_to_wait = rospy.Time.now() - self._start_time - self._target_time ##if time_to_wait > 0: ##Logger.loginfo('Need to wait for %.1f seconds.' % time_to_wait) goal = MoveBaseActionGoal() goal.header.stamp = Time.now() goal.header.frame_id = userdata.frameId goal.goal.target_pose.pose.position = userdata.pose.position goal.goal.distance = userdata.params['distance'] # "straighten up" given orientation yaw = euler_from_quaternion([userdata.pose.orientation.x, userdata.pose.orientation.y, userdata.pose.orientation.z, self.pose.orientation.w])[2] goal.goal.target_pose.pose.orientation.x = 0 goal.goal.target_pose.pose.orientation.y = 0 goal.goal.target_pose.pose.orientation.z = math.sin(yaw/2) goal.goal.target_pose.pose.orientation.w = math.cos(yaw/2) goal.goal.target_pose.header.frame_id = userdata.frameId goal.goal_id.id = 'driveTo' + str(goal.header.stamp.secs) + '.' + str(goal.header.stamp.nsecs) goal.goal_id.stamp = goal.header.stamp userdata.goalId = goal.goal_id.id self._pub.publish(self._topic_move_base_goal, goal) return 'succeeded'
def callback_simple_goal(self, goal): rospy.logdebug('[Move Base Fake] Simple Goal: {}'.format( str(goal.pose.position))) action_goal = MoveBaseActionGoal() action_goal.header.stamp = rospy.Time.now() action_goal.goal.target_pose = goal self.goal_pub.publish(action_goal)
def goTo(name): # TODO: Implement this. global navPending global needsCancel global callback navPending = True if name == 'lower torso': torso = fetch_api.Torso() torso.set_height(0.0) navPending = False return 0 elif name == 'raise torso': torso = fetch_api.Torso() torso.set_height(0.4) navPending = False return 0 else: if not os.path.exists('pickle/' + name): print 'No such pose \'' + name + '\'' navPending = False return 1 else: loadfile = open('pickle/' + name, 'rb') stampedCoPose = pickle.load(loadfile) loadfile.close() # TODO: Figure out why this goal doesn't cause any motion mbagoal = MoveBaseActionGoal() mbgoal = MoveBaseGoal() mbgoal.target_pose.header.frame_id = 'map' mbgoal.target_pose.header.stamp = rospy.Time().now() mbgoal.target_pose.pose = stampedCoPose.pose #potential issue here mbagoal.goal = mbgoal #mbagoal.header = #mbagoal.goal_id.id = 'ios' pub.publish(mbagoal) #pub.send_goal(mbgoal) # END TODO rospy.sleep(5) while callback.motion: print 'waiting' rospy.sleep(1) navPending = False callback.motion = True return 0 navPending = False return 1
def move_to_goal(self, goal): mbag = MoveBaseActionGoal() # First Header mbag.header.frame_id = '' # Use the map frame to define goal poses mbag.header.seq = self.goal_id mbag.header.stamp = rospy.Time.now() # Now the Goal ID mbag.goal_id.stamp = rospy.Time.now() mbag.goal_id.id = "g_" + str(self.goal_id) # Now the goal, which we already have as an argument mbag.goal = goal #PoseStamped(Pose(Point(2.0, -3.0, 0.0), quaternion_from_euler(0, 0, -1.5706, axes='sxyz'))) self.goal_setter.publish(mbag)
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 move(self, x, y, a): goal = MoveBaseActionGoal() goal.goal.target_pose.header.frame_id = 'odom' goal.goal.target_pose.pose.position.x = x goal.goal.target_pose.pose.position.y = y orient = tf.transformations.quaternion_from_euler(0, 0, a) goal.goal.target_pose.pose.orientation.z = orient[2] goal.goal.target_pose.pose.orientation.w = orient[3] self.pub_goal.publish(goal)
def colour_move(self, x, z): colour_found = MoveBaseActionGoal() colour_found.goal.target_pose.pose.position.x = x colour_found.goal.target_pose.pose.orientation.z = z colour_found.goal.target_pose.pose.orientation.w = 1 colour_found.goal.target_pose.header.stamp = rospy.Time.now() colour_found.goal.target_pose.header.frame_id = 'base_link' colour_found.goal.target_pose.header.seq = 101 self.movePub.publish(colour_found)
def move(self, x, y): co_move = MoveBaseActionGoal() co_move.goal.target_pose.pose.position.x = x co_move.goal.target_pose.pose.position.y = y co_move.goal.target_pose.pose.orientation.w = 1 co_move.goal.target_pose.header.stamp = rospy.Time.now() co_move.goal.target_pose.header.frame_id = 'map' co_move.goal.target_pose.header.seq = 102 self.movePub.publish(co_move)
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 talker(self,data): numPath = len(data.poses) for i in range(numPath): pub_msg = MoveBaseActionGoal() pub_msg.goal.target_pose = data.poses[i] pub_msg.header = data.header pub_msg.goal.target_pose.header.frame_id = 'map' pub_msg.header.frame_id = 'map' # pub_msg.goal.target_pose.header.frame_id = data.poses[i].header.frame_id pub_msg.goal.target_pose.pose.position.x = data.poses[i].pose.position.x-2 pub_msg.goal.target_pose.pose.position.y = data.poses[i].pose.position.y-2 pub_msg.goal.target_pose.pose.position.z = data.poses[i].pose.position.z # pub_msg.goal.target_pose.pose.orientation = data.poses[i].pose.orientation if(self.RecFlag): self.pub.publish(pub_msg) rospy.sleep(2) print(pub_msg) # flag=True self.RecFlag=False
def __init__(self, parameters): self.parameters = parameters self.ssne_param = self.parameters.ssne_param self.num_input = self.ssne_param.num_input self.num_hidden = self.ssne_param.num_hnodes self.num_output = self.ssne_param.num_output if self.parameters.arch_type == 'QUASI_GRUMB': self.ssne = mod.Quasi_GRUMB_SSNE(parameters) else: self.ssne = mod.Fast_SSNE(parameters) #nitialize SSNE engine #####Initiate the agent self.agent = Agent_Pop(parameters, 0) #ROS stuff self.oracle_x = [[-1, -1, -1], [-1, -1, 1], [-1, 1, -1], [-1, 1, 1], [1, -1, -1], [1, -1, 1], [1, 1, -1], [1, 1, 1]] self.oracle_y = [[3.348, 0.955], [0.550, 1.051], [0.546, 8.904], [3.299, 8.929], [6.583, 9.0120], [9.087, 8.966], [9.316, 0.897], [6.569, 0.926]] self.oracle_goals = self.format_goal(self.oracle_y) self.home_coordinates = [[5.0, 4.0]] self.home_goal = self.format_goal(self.home_coordinates) rospy.init_node('Evolve_GRUMB', anonymous=True) # Node self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.update_scan) # self.cmd_sub = rospy.Subscriber('/navigation_velocity_smoother/raw_cmd_vel', Twist, self.update_command) self.goal_reached_sub = rospy.Subscriber('/move_base/status', GoalStatusArray, self.update_goal_status) self.odom_sub = rospy.Subscriber('/base_pose_ground_truth', Odometry, self.update_odom) self.goal_pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=1) self.goal_pub.publish(MoveBaseActionGoal()) time.sleep(0.3) # Dummy hack self.pub = rospy.Publisher( '/navigation_velocity_smoother/raw_cmd_vel', Twist, queue_size=10) # Publisher to command velocity # Control Action defined and initialized self.action = Twist() self.action.linear.x = 0.0 self.action.angular.z = 0.0 self.action.linear.y = 0.0 # Define max applicable to robot self.max_speed = 1.0 self.is_pursuing = False self.pursue_status = 1 self.scan_readings = None self.base_odom = Pose()
def slam_move(pose): goal = MoveBaseActionGoal() goal.goal.target_pose.header.frame_id = 'map' goal.goal.target_pose.pose.position.x = pose[0] goal.goal.target_pose.pose.position.y = pose[1] goal.goal.target_pose.pose.position.z = 0.0 goal.goal.target_pose.pose.orientation.x = 0 goal.goal.target_pose.pose.orientation.y = 0 goal.goal.target_pose.pose.orientation.z = math.sin(pose[2] / 2) goal.goal.target_pose.pose.orientation.w = math.cos(pose[2] / 2) slam_move_pub.publish(goal)
def pubGoal(x, y): pub_msg = MoveBaseActionGoal() # pub_msg.header = data.header pub_msg.goal.target_pose.header.frame_id = 'map' pub_msg.header.frame_id = 'map' # pub_msg.goal.target_pose.header.frame_id = data.poses[i].header.frame_id pub_msg.goal.target_pose.pose.position.x = x pub_msg.goal.target_pose.pose.position.y = y pub_msg.goal.target_pose.pose.position.z = 0 print(pub_msg) pub.publish(pub_msg)
def set_target(): global pub_target,desired_position_ Goal = MoveBaseActionGoal() Goal.goal.target_pose.header.frame_id = "map" Goal.goal.target_pose.pose.orientation.w =1 Goal.goal.target_pose.pose.position.x = rospy.get_param("des_pos_x") Goal.goal.target_pose.pose.position.y = rospy.get_param("des_pos_y") pub_target.publish(Goal) print("Published new target!") desired_position_.x = rospy.get_param("des_pos_x") desired_position_.y = rospy.get_param("des_pos_y")
def get_goals(self, file): # Read in the next goal pose. Set it up, return to send goal method. #FORMAT: #x, y; run_identfier = time.time() goal_list = [] clockwise = True if run_identfier % 2 == 0 else False run_identfier = str( run_identfier) + "_clock" if clockwise else "_counterclock" with open(file, 'r') as goals_file: goals_reader = csv.reader(goals_file, delimiter=',') # for i in range(5): for row_num, row in enumerate(goals_reader): MBAG = MoveBaseActionGoal() MBAG.goal_id = GoalID() MBAG.goal_id.id = str(run_identfier) + "_path_" + str(row_num) MBAG.header = Header() MBAG.header.frame_id = 'map' MBG = MoveBaseGoal() MBG.target_pose.header.frame_id = "map" MBG.target_pose.pose.position.x = float(row[0]) MBG.target_pose.pose.position.y = float(row[1]) MBG.target_pose.pose.orientation.w = 1.0 MBAG.goal = MBG goal_list.append(MBAG) #Switch directions half the time if clockwise: temp = goal_list[0] goal_list[0] = goal_list[2] goal_list[2] = temp rospy.loginfo("Pathing clockwise: " + str(clockwise)) return goal_list
def publishNextPoint(self): x_next, y_next = self.convert_node_to_point(self.next_node) w_next = self.computeNextOrientation(x_next,y_next) rospy.loginfo("[%s] Converted next node to point: (%s,%s), orientation: %s" %(self.node_name, x_next, y_next, w_next)) goal_id = GoalID() goal_id.id = str(self.goal_idx) msg = MoveBaseActionGoal() msg.goal_id = goal_id msg.goal.target_pose.header.frame_id = self.frame_id msg.goal.target_pose.pose.position.x = x_next msg.goal.target_pose.pose.position.y = y_next msg.goal.target_pose.pose.position.z = 0 msg.goal.target_pose.pose.orientation.x = 0 msg.goal.target_pose.pose.orientation.y = 0 msg.goal.target_pose.pose.orientation.z = math.sin(w_next/2) msg.goal.target_pose.pose.orientation.w = math.cos(w_next/2) self.pub_goal.publish(msg) rospy.loginfo("[%s] Dispatched goal point: (%s,%s)" %(self.node_name,x_next,y_next)) if self.next_node == self.goal: self.dispatched_end = True
def sendToOnce(): global PUB_GOAL, PUBD # if PUBD: # return # PUBD = True DISTANCE_THRESHOLD = .1 print'********' # x = data.pose.pose.position.x # y = data.pose.pose.position.y # if math.sqrt(x**2+y**2) < DISTANCE_THRESHOLD: # return ps = PoseStamped() ps.header.frame_id = "/map" ps.header.stamp = rospy.Time.now() ps.pose.position.x = 1 ps.pose.position.y = 0 ps.pose.position.z = 0 q = quaternion_from_euler(0, 0, 0, 'sxyz') ps.pose.orientation.x = q[0] ps.pose.orientation.y = q[1] ps.pose.orientation.z = q[2] ps.pose.orientation.w = q[3] mbag = MoveBaseActionGoal() mbag.header = ps.header mbag.goal.target_pose = ps print mbag PUB_GOAL.publish(mbag)
def publish_goal(): global centroids global finished global goal_counter global failed global full_fail global last_goal global last_none_flag global goal global init init= PoseStamped() init.pose.position.x=0 init.pose.position.y=0 init.pose.position.z=0 init.pose.orientation.x=0 init.pose.orientation.y=0 init.pose.orientation.z=1 goal_counter += 1 # print "dajkhfjajlfjdlka" if len(centroids) > 0: print "Publishing goal %s" % goal_counter # get the first centroid first = centroids.pop(0) # check if it is too close dist = math.sqrt((first.pose.position.x - newPose.pose.position.x)**2 + (first.pose.position.y - newPose.pose.position.y)**2) # if last_goal != None: # last_none_flag = 0 # last_dist = 999 # if not last_none_flag: # last_dist = math.sqrt((last_goal.pose.position.x - newPose.pose.position.x)**2 + (last_goal.pose.position.y - newPose.pose.position.y)**2) # create a goal goal = MoveBaseActionGoal() goal.header = first.header goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = first goal_pub.publish(goal) last_goal = first # else: # print "Goal too close" # # goal_counter -= 1 # if len(centroids) > 0: # publish_goal() # elif full_fail > 3: # finished = 1 # else: # full_fail += 1 # run_navigation(False) else: # there are no reachable centroids if failed: finished = 1 goal = MoveBaseActionGoal() goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = init goal_pub.publish(goal) else: failed = 1 goal = MoveBaseActionGoal() goal.goal_id.id = "Goal%s" % goal_counter goal.goal.target_pose = init goal_pub.publish(goal) run_navigation(False)