Esempio n. 1
0
    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
Esempio n. 2
0
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())
Esempio n. 3
0
    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)
Esempio n. 4
0
    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
Esempio n. 6
0
def cancel_command():
    cancelmessage = GoalID()
    cancelmessage.stamp.secs = 0
    cancelmessage.stamp.nsecs = 0
    cancelmessage.id = ''
    cancel_cmd.publish(cancelmessage)
    print("TestFunction")
Esempio n. 7
0
 def shutdown(self):
     try:
         for x in self.leds.pins:
             wiringpi2.digitalWrite(x, 0)
         self.cancel_pub.publish(GoalID())
     except:
         pass
Esempio n. 8
0
    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())
Esempio n. 9
0
 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)
Esempio n. 10
0
 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())
Esempio n. 11
0
 def _getPath(self, path):
     if self.calculate_path == True:
         self.pubStopNav.publish(GoalID())
         self.path = path
         print("Path=", path)
     else:
         pass
Esempio n. 12
0
    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)
Esempio n. 13
0
 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))
             )
Esempio n. 14
0
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)
Esempio n. 15
0
 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
Esempio n. 16
0
    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
Esempio n. 17
0
    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()
Esempio n. 18
0
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)
Esempio n. 19
0
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
Esempio n. 20
0
 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)
Esempio n. 23
0
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
Esempio n. 24
0
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()
Esempio n. 25
0
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
Esempio n. 26
0
 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)
Esempio n. 27
0
 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)
Esempio n. 28
0
 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...")
Esempio n. 29
0
    def __init__(self):
        self.goalid1 = GoalID()
        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        self.goal = MoveBaseGoal()
        self.id_goal = []
        self.Stop = True

        pass
Esempio n. 30
0
 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())