def callback_main(self, data): if not self.stop: if data.x != -1: self.flag_find = True if self.flag_explore and self.status_explore_goal == 1 and data.y <= 30: rospy.loginfo("Stop Explore and kill Operator") rospy.Publisher("/Explore/cancel", GoalID, queue_size=1).publish(GoalID()) os.system("rosnode kill /Operator") time.sleep(5) self.flag_explore = False if (self.move_base_info.status_list and self.move_base_info.status_list[0].status == 1) and data.y <= 4: rospy.Publisher('/move_base/cancel', GoalID, queue_size=1).publish(GoalID()) self.goal_ajustment(data) elif not self.flag_explore: self.move_goal_to_object(data.x, data.z) else: if not self.flag_find and not self.flag_explore and self.status_explore_goal != 1: rospy.loginfo("Wait..") time.sleep(5) self.pub_explore_goal.publish(ExploreActionGoal()) rospy.loginfo("Start Explore") self.flag_explore = True
def odom_callback(msg): global OLD_MAP, currentX, currentY, currentAngle, last_refreshed, visited, currentRow, currentCol, thinking # find current (x,y) position of robot based on odometry currentX = msg.pose.pose.position.x currentY = msg.pose.pose.position.y # find current orientation of robot based on odometry (quaternion coordinates) xOr = msg.pose.pose.orientation.x yOr = msg.pose.pose.orientation.y zOr = msg.pose.pose.orientation.z wOr = msg.pose.pose.orientation.w # find orientation of robot (Euler coordinates) (roll, pitch, yaw) = euler_from_quaternion([xOr, yOr, zOr, wOr]) # find currentAngle of robot (equivalent to yaw) # now that you have yaw, the robot's pose is completely defined by (currentX, currentY, currentAngle) currentAngle = yaw if OLD_MAP is None: return # cancel the current waypoint and assign a new one if we aren't moving if not thinking and sitting_still(): backup(400) clear_pub.publish(Empty()) cancel_pub.publish(GoalID()) currentRow, currentCol = utils.coord_to_row_col((currentX, currentY), RESOLUTION, MAP) # mark the area around the region we are in as visited visited[currentRow - visit_width:currentRow + visit_width, currentCol - visit_width:currentCol + visit_width] = 1 # check if we are in a region that was unexplored when we # originally chose this waypoint. If so, the current # path might take us through a wall, so it is better to # cancel the waypoint and reroute. refresh_rate = 10 if (OLD_MAP[currentRow-7:currentRow+8, currentCol-7:currentCol+8] == -1).all() \ and time.time() - last_refreshed > refresh_rate: print("Reassessing route...") last_refreshed = time.time() backup(400) clear_pub.publish(Empty()) cancel_pub.publish(GoalID())
def stop(self): # Cancel all current goals cancel_pub = rospy.Publisher(rospy.get_name() + '/cancel', GoalID) cancel_pub.publish(GoalID(stamp=rospy.Time.now())) # Wait a bit rospy.sleep(0.5)
def __init__(self): """ JARVIS User Interface Node """ rospy.on_shutdown(self.cleanup) rospy.wait_for_service('return_grips') self.msg = GoalID() self.newGrips = jarvis_perception.msg.GraspArray() self.legalUtterances = [ 'upper', 'lower', 'top', 'bottom', 'back', 'front', 'near', 'far', 'rear', 'left', 'right', 'anywhere', 'center' ] self.lastUtterance = '' self.axisAlignedBox = '' self.person_trans = '' self.person_rot = '' plotting = True self.likelihood = None self.var = None try: grip_server = rospy.ServiceProxy('return_grips', ReturnGrips) self.grips = grip_server() print self.grips pubgrips = rospy.Publisher('return_grips', jarvis_perception.msg.GraspArray, queue_size=10) except rospy.ServiceException, e: print "Service call failed: %s" % e
def goalCallback(self, msg): self.goal_pose = msg.pose while True: # print("robot pose: {}, {}".format(self.robot_pose.position.x, self.robot_pose.position.y)) # print("goal pose: {}, {}".format(self.goal_pose.position.x, self.goal_pose.position.y)) self.distance = sqrt( pow(self.goal_pose.position.x - self.robot_pose.position.x, 2) + pow(self.goal_pose.position.y - self.robot_pose.position.y, 2)) print(self.distance) if self.first_time == False: self.end_time = time.time() if self.distance < self.offset: if self.first_time == True: self.start_time = time.time() self.first_time = False if self.first_time != None and self.end_time != None: self.interval_time = abs(self.end_time - self.start_time) print("TIME START: {}".format(self.start_time)) print("END TIME: {}".format(self.end_time)) print("INTERVAL TIME: {}".format(self.interval_time)) if self.interval_time >= self.stuck_time: cancel_pub = rospy.Publisher("/move_base/cancel", GoalID, queue_size=1) cancel_msg = GoalID() cancel_pub.publish(cancel_msg) rospy.sleep(1) rospy.loginfo("goal has been canceled") break
def cancel_command(): cancelmessage = GoalID() cancelmessage.stamp.secs = 0 cancelmessage.stamp.nsecs = 0 cancelmessage.id = '' cancel_cmd.publish(cancelmessage) print("TestFunction")
def shutdown(self): try: for x in self.leds.pins: wiringpi2.digitalWrite(x, 0) self.cancel_pub.publish(GoalID()) except: pass
def __init__(self): rospy.loginfo("INIT CONTROL VISION") rospy.init_node("robot_vision", anonymous=True) self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0) self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0) self.flag_find = False self.flag_explore = False self.stop = False self.time_start = time.time() rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo, self.callback_camera_info) rospy.Subscriber("/move_base/status", GoalStatusArray, self.callback_move_base_info) rospy.Subscriber("/Explore/status", GoalStatusArray, self.callback_explore_status) self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1) self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=1) self.pub_first_map_goal = rospy.Publisher("/GetFirstMap/goal", GetFirstMapActionGoal, queue_size=1) self.pub_explore_goal = rospy.Publisher("/Explore/goal", ExploreActionGoal, queue_size=1) cancel_first_map = rospy.Publisher("/GetFirstMap/cancel", GoalID, queue_size=1) time.sleep(1) self.pub_first_map_goal.publish() time.sleep(1) cancel_first_map.publish(GoalID())
def __init__(self, name, user): self.user = user self.timer, self.delta = 0., time.clock() self.poses, self.gids = [], [] self.status = 0 self.name = name self.key = 'moving' # Crear mensaje PoseStamped self.pose = PoseStamped() self.pose.header.stamp = rospy.Time.now() self.pose.header.frame_id = "map" self.pose.pose.position.x = 0 self.pose.pose.position.y = 0 self.pose.pose.position.z = 0 self.pose.pose.orientation.x = 0 self.pose.pose.orientation.y = 0 self.pose.pose.orientation.z = 0 self.pose.pose.orientation.w = 1 self.poses.append(self.pose) self.pub = rospy.Publisher('/%s/move_base_simple/goal' % self.name, PoseStamped, queue_size=100) self.cancel = GoalID() self.cancel.stamp = rospy.Time.now() self.cancel.id = '/%s/HOME' % self.name self.gids.append(self.cancel) self.pub_cancel = rospy.Publisher('/%s/move_base/cancel' % self.name, GoalID, queue_size=100) self.subs = rospy.Subscriber('/%s/move_base/status' % self.name, GoalStatusArray, self.callbackStatus) self.subs_goal = rospy.Subscriber('/%s/move_base/goal' % self.name, MoveBaseActionGoal, self.newGoalID) self.subs_img = rospy.Subscriber( '/%s/camera/rgb/image_raw' % self.name, Image, self.newImage)
def __init__(self, **kwargs): assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \ "Invalid arguments passed to constructor: %r" % kwargs.keys() from actionlib_msgs.msg import GoalID self.goal_id = kwargs.get('goal_id', GoalID()) self.status = kwargs.get('status', int()) self.text = kwargs.get('text', str())
def _getPath(self, path): if self.calculate_path == True: self.pubStopNav.publish(GoalID()) self.path = path print("Path=", path) else: pass
def schedule(self, command: TrajectoryCommand): """Schedules a new trajectory. :param TrajectoryCommand command: The trajectory command to schedule """ self._failed = False stamp = command.start_time.to_msg() command.trajectory.header.stamp = stamp goal = FollowJointTrajectoryGoal(trajectory=command.trajectory) self._trajectory_goal_pub.publish( FollowJointTrajectoryActionGoal( header=Header(stamp=stamp), goal_id=GoalID(stamp=stamp, id=str(command)), goal=goal, ) ) info_log_message = f"Scheduling {command.name}" debug_log_message = f"Subgait {command.name} starts " if self._node.get_clock().now() < command.start_time: time_difference = Duration.from_ros_duration( command.start_time - self._node.get_clock().now() ) debug_log_message += f"in {round(time_difference.seconds, 3)}s" else: debug_log_message += "now" self._goals.append(command) self.logger.info(info_log_message) self.logger.debug(debug_log_message)
def cancel_active_goals(self): now = self._node.get_clock().now() for goal in self._goals: if goal.start_time + goal.duration > now: self._cancel_pub.publish( GoalID(stamp=goal.start_time.to_msg(), id=str(goal)) )
def process_data(conn, c): """Processes incoming data from socket `conn` and performs the corresponding action using the attributes from container `c` """ data = conn.recv(BUFFER_SIZE) while data: data = json.loads(data) if data['name'] == 'goal_map': message = PoseStamped() create_pose_msg(data, c.seq_goal, message) message.header.frame_id = 'map' c.pub_goal.publish(message) c.seq_goal += 1 elif data['name'] == 'initial': message = PoseWithCovarianceStamped() create_pose_msg(data, c.seq_initial, message) message.header.frame_id = 'map' message.pose.covariance = FIXED_COVARIANCE c.pub_initial.publish(message) c.seq_initial += 1 elif data['name'] == 'get_pose': pose = read_recent_pose(c.listener_pose) conn.sendall(pose) elif data['name'] == 'goal_robot': message = PoseStamped() create_pose_msg(data, c.seq_goal, message) message.header.frame_id = 'base_link' c.pub_goal.publish(message) c.seq_goal += 1 elif data['name'] == 'cancel': c.pub_cancel.publish(GoalID()) data = conn.recv(BUFFER_SIZE)
def stop_navigation(self): if self.navigation_goal_id == '-1': # only cancel nav if currently underway return msg = GoalID() # init cancel message msg.id = self.navigation_goal_id # specify current goal id self.move_cancel_pub.publish(msg) # publish message self.navigation_goal_id = '-1' # update current nav goal id
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 __init__(self, node_name, topic_1 = None, msg_type_1 = None, send_topic_1 = None, topic_2 = None, msg_type_2 = None, send_topic_2 = None): self._motive_topic_1=node_name+'/activate' if not(topic_1 == None) and not(msg_type_1 == None): self._topic_1 = topic_1 self._msg_type_1 = msg_type_1 self._send_topic_1=send_topic_1 self.send_1 = FunctionUnit.create_send(self, self._send_topic_1, self._msg_type_1) else: self._topic_1 = None self._msg_type_1 = None if not(topic_2 == None) and not(msg_type_2 == None): self._topic_2 = topic_2 self._msg_type_2 = msg_type_2 self._send_topic_2=send_topic_2 self.send_2 = FunctionUnit.create_send(self, self._send_topic_2, self._msg_type_2) else: self._topic_2 = None #print 'init' FunctionUnit.__init__(self, node_name) self.motivational_shared_var = False #'True' means the behavior set is activated self._action_mode = False self._last_goal = GoalID()
def cancel_move_base(): print("canceled move base") #cancel action pubCancelAction = rospy.Publisher('/move_base/cancel', GoalID, queue_size=0) answerCancel = GoalID() answerCancel.stamp = rospy.get_rostime() answerCancel.id = "map" pubCancelAction.publish(answerCancel) #cancel movement pubCancelMove = rospy.Publisher('cmd_vel', Twist, queue_size=10) twist = Twist() twist.linear.x = 0.0 twist.linear.y = 0.0 twist.linear.z = 0.0 twist.angular.x = 0.0 twist.angular.y = 0.0 twist.angular.z = 0.0 pubCancelMove.publish(twist)
def callback(msg): global stopMessage global goHome global wallFollow global isWallFollow message = msg.data (tempStop, tempGoHome, tempWallFollow) = message.split(":") #print(tempStop, tempGoHome, tempWallFollow) if int(tempStop) != int(stopMessage): # Stop wall follow isWallFollow = False cmd_pub.publish("stop") cancel_nav_pub.publish(GoalID()) tempTwist = Twist() tempTwist.linear.x = 0 tempTwist.linear.y = 0 tempTwist.linear.z = 0 tempTwist.angular.x = 0 tempTwist.angular.y = 0 tempTwist.angular.z = 0 cmd_vel_pub.publish(tempTwist) print("Sending stop movement",tempStop, stopMessage) elif (int(tempGoHome) != int(goHome)): # Go home beacon_pub.publish("1") print("Sending navigate home", goHome, tempGoHome) elif (int(tempWallFollow) != int(wallFollow)) and (isWallFollow == False): # Start wall follow isWallFollow = True print("lSending run wall follow", tempWallFollow, wallFollow) cmd_pub.publish("start") wallFollow = tempWallFollow goHome = tempGoHome stopMessage = tempStop
def _generate_id(self): global g_goal_id id, g_goal_id = g_goal_id, g_goal_id + 1 now = rospy.Time.now() return GoalID(id="%s-%i-%.3f" % (rospy.get_caller_id(), id, now.to_sec()), stamp=now)
def callback(self, data): if data.x != -1: if not self.move_base_info.status_list and self.flag_orientation: self.orientation_to_obj(data) elif self.flag_move_to_goal: self.publisher_move_to_goal(data) self.flag_move_to_goal = False self.flag_ajustment = True self.flag_find = True elif (self.move_base_info.status_list and self.move_base_info.status_list[0].status != 1 ) and self.flag_ajustment: self.goal_ajustment(data) if (self.move_base_info.status_list and self.move_base_info.status_list[0].status == 1) and data.y <= 4: rospy.Publisher('/move_base/cancel', GoalID, queue_size=1).publish(GoalID()) else: if self.flag_find and ( self.move_base_info.status_list and self.move_base_info.status_list[0].status != 1): self.msg_twist.angular.z = 0.5 self.pub_cmd_vel.publish(self.msg_twist.angular.z)
def on_joy(self, data): # Set speed ratio using d-pad twist = Twist() if data.axes[7] == 1: # moving forward twist.linear.x = 1 elif data.axes[7] == -1: twist.linear.x = -1 if data.axes[6] == 1: # rotating left twist.angular.z = 1 elif data.axes[6] == -1: twist.angular.z = -1 # Stop if data.buttons[1]: # B button twist.linear.x = 0 twist.angular.z = 0 # Publish Twist self.cmd_vel_pub.publish(twist) # Cancel move base goal if data.buttons[2]: # X button rospy.loginfo('Cancelling move_base goal') cancel_msg = GoalID() self.goal_cancel_pub.publish(cancel_msg)
def getGoal(data): #print("get goal!") goal.x = data.pose.position.x goal.y = data.pose.position.y msg = GoalID() pubStopMovebase.publish(msg) receiveGoal = True
def main(): global position_,srv_client_get_random_,pub,active_,pub2,counter2,change,state_ #varibles counter2 is used to publish a random position when service is activated counter2=True #init a new node rospy.init_node('move_randomly') #service for move randomly behaviour srv = rospy.Service('move_randomly', SetBool, move_randomly) #service client for getting a random position srv_client_get_random_ = rospy.ServiceProxy('/random_service', Empty) #publisher on /move_base/goal for reaching a new position pub = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=1) #publisher on /move_base/cancel for cancel the goal once that is reached pub2 = rospy.Publisher('/move_base/cancel', GoalID, queue_size=1) #subscriber of topic /odom for getting actual position sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom) rate = rospy.Rate(20) while not rospy.is_shutdown(): #if service is active if active_: #if the service has been just activate ask for a random position and publish on /move_base if counter2: resp = srv_client_get_random_() pub_goal() counter2=False msg_cancel=GoalID() a=1 a=distance() #take the value of distance from robot to goal #when distance is smaller then 0.5 the goal is reached if a<0.5 : print('GOAL REACHED') change=rospy.get_param("change") state_=rospy.get_param("state") #if the status of the robot has not been changed when robot was reaching the target publish a new random target if change==0: pub2.publish(msg_cancel) resp = srv_client_get_random_() pub_goal() #if the status has not been changed by robot disable this behaviour else: pub2.publish(msg_cancel) active_=False else: counter2=True rate.sleep()
def getGoal(data): #print("get goal!") goal.x = data.pose.position.x goal.y = data.pose.position.y #print("goal:{} {}".format(goal.x, goal.y)) msg = GoalID() pubStopMovebase.publish(msg) receiveGoal = True
def cancel(self): with self.comm_state_machine.mutex: cancel_msg = GoalID( stamp=rospy.Time(0), id=self.comm_state_machine.action_goal.goal_id.id) self.comm_state_machine.send_cancel_fn(cancel_msg) self.comm_state_machine.transition_to( CommState.WAITING_FOR_CANCEL_ACK)
def click_cancel_cb(self, msg): if msg.data: print "clicked cancel" for i in range(3): cancel_msg = GoalID() cancel_msg.id = self.last_goal_id cancel_msg.stamp = rospy.Time.now() self.mb_cancel_pub.publish(cancel_msg)
def cancel_exploration(self): if self.exploration_id: rospy.logerr("Robot {} Cancelling exploration...".format(self.robot_id)) goal_id = GoalID() goal_id.id = self.exploration_id self.cancel_explore_pub.publish(goal_id) else: rospy.logerr("Exploration ID not set...")
def __init__(self): self.goalid1 = GoalID() self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction) self.goal = MoveBaseGoal() self.id_goal = [] self.Stop = True pass
def __init__(self, **kwargs): assert all('_' + key in self.__slots__ for key in kwargs.keys()), \ 'Invalid arguments passed to constructor: %s' % \ ', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__)) from actionlib_msgs.msg import GoalID self.goal_id = kwargs.get('goal_id', GoalID()) self.status = kwargs.get('status', int()) self.text = kwargs.get('text', str())