Esempio n. 1
0
def cancel_command():
    cancelmessage = GoalID()
    cancelmessage.stamp.secs = 0
    cancelmessage.stamp.nsecs = 0
    cancelmessage.id = ''
    cancel_cmd.publish(cancelmessage)
    print("TestFunction")
Esempio n. 2
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. 3
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
	def on_enter(self, userdata):

		self._startTime = Time.now()

		self._failed = False
		self._reached = False
		
		goal_id = GoalID()
		goal_id.id = 'abcd'
		goal_id.stamp = Time.now()

		action_goal = MoveBaseGoal()
		action_goal.target_pose = userdata.waypoint
		action_goal.speed = userdata.speed

		if action_goal.target_pose.header.frame_id == "":
			action_goal.target_pose.header.frame_id = "world"

		try:
			self._move_client.send_goal(self._action_topic, action_goal)
			resp = self.set_tolerance(goal_id, 0.2, 1.55)
		except Exception as e:
			Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
				'err': str(e),
				'x': userdata.waypoint.pose.position.x,
				'y': userdata.waypoint.pose.position.y
			})
			self._failed = True
		
		Logger.loginfo('Driving to next waypoint')
Esempio n. 5
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. 6
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. 7
0
 def new_goal_id(self):
     now = rospy.get_rostime()
     new_id = "%s_%i_%i_%i" % (rospy.get_name(), self.count, now.secs, now.nsecs)
     self.count += 1
     goal_id = GoalID()
     goal_id.stamp = now
     goal_id.id = new_id
     return goal_id
Esempio n. 8
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...")
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
Esempio n. 10
0
 def new_goal_id(self):
     now = rospy.get_rostime()
     new_id = "%s_%i_%i_%i" % (rospy.get_name(), self.count, now.secs,
                               now.nsecs)
     self.count += 1
     goal_id = GoalID()
     goal_id.stamp = now
     goal_id.id = new_id
     return goal_id
Esempio n. 11
0
    def execute(self, userdata):
        rospy.loginfo('Executing Park')
        rospy.loginfo('STAYING HERE')
        goal_id = GoalID()
        goal_id.id = ''
        pub = rospy.Publisher('/move_base/cancel', GoalID, queue_size=10)
        pub.publish(goal_id)

        return 'parkSuccessful'
    def prepare_actionlib_msg(self):
        '''
        Fill action lib message
        '''
        goal_id_msg = GoalID()

        now = rospy.get_rostime()
        goal_id_msg.stamp = now
        goal_id_msg.id = "ACTIVE"
    def prepare_and_publish_actionlib_msg(self):
        '''
        Fill and publish action lib message
        '''
        goal_id_msg = GoalID()

        now = rospy.get_rostime()
        goal_id_msg.stamp = now
        goal_id_msg.id = "ACTIVE"
Esempio n. 14
0
 def cancel_exploration(self):
     if self.exploration_id:
         pu.log_msg(self.robot_id, "Cancelling exploration...",
                    self.debug_mode)
         goal_id = GoalID()
         goal_id.id = self.exploration_id
         self.cancel_explore_pub.publish(goal_id)
     else:
         pu.log_msg(self.robot_id, "Exploration ID not set...",
                    self.debug_mode)
Esempio n. 15
0
 def PrepareToStop(self, data):
     if self.object_x and self.object_y and self.moving and np.sqrt(
             np.square(self.object_x - data.pose.pose.position.x) +
             np.square(self.object_y - data.pose.pose.position.y)) < 0.5:
         cancel_msg = GoalID()
         cancel_msg.id = ""
         cancel_msg.stamp = rospy.Time.now()
         self.cancel_publisher.publish(cancel_msg)
         self.moving = False
         self.object_x = None
         self.object_y = None
 def run(self):
     pub = rospy.Publisher('/Mock/Dobot_Loader/suctionStatus', GoalID, queue_size=10)
     rospy.init_node('AddDetectedObjectServer')
     r = rospy.Rate(10) # 10hz
     while not rospy.is_shutdown():
         self.listener()
         my_message = GoalID()
         my_message.id=str(self.current_value)
         my_message.stamp.nsecs=int(self.current_value)
         my_message.stamp.secs=int(self.current_value)
         pub.publish(my_message)
         r.sleep()
 def cancel_goal(self):
     goal_id = GoalID()
     goal_id.id = self.last_published_goal.goal_id.id
     self.goal_cancel_publisher.publish(goal_id)
     self.is_moving = False
     # print '\nGoal canceled...'
     if self.tour_started:
         tts.speak('Tour cancelled')
         self.tour_started = False
     else:
         tts.speak('Goal canceled')
     time.sleep(5)
     # print '\nReturning to waiting position...'
     tts.speak('Returning to waiting position')
     time.sleep(2)
     self.publish_goal(self.start_location)
Esempio n. 18
0
 def stop_movement(self, timeout=5.0):
     '''
     description: cancels the goal currently being pursued
     needs: mir_move_base_safe and mbot_actions move_base_server
     input: tdouble timeout, max time to allow to reach the location before returning
     output: bool success, whether the cancelation was successful or not
     '''
     cancel_msg = GoalID()
     cancel_msg.stamp = rospy.Time.now()
     if self.__goals_status:
         for goal in self.__goals_status:
             if goal.status == 0 or goal.status == 1:
                 goal_id = goal.goal_id
                 cancel_msg.id = goal_id
                 self.__cancel_pub.publish(cancel_msg)
                 return True
     return False
Esempio n. 19
0
    def generate_ID(self):
        """
        * \brief Generates a unique ID
        * \return A unique GoalID for this action
        """
        id = GoalID()
        cur_time = rospy.Time.now()
        ss = self.name + "-"
        global s_goalcount_lock
        global s_goalcount
        with s_goalcount_lock:
            s_goalcount += 1
            ss += str(s_goalcount) + "-"
        ss += str(cur_time.secs) + "." + str(cur_time.nsecs)

        id.id = ss
        id.stamp = cur_time
        return id
Esempio n. 20
0
    def generate_ID(self):
        """
        * \brief Generates a unique ID
        * \return A unique GoalID for this action
        """
        id = GoalID();
        cur_time = rospy.Time.now();
        ss = self.name +  "-";
        global s_goalcount_lock
        global s_goalcount
        with s_goalcount_lock:
            s_goalcount += 1
            ss += str(s_goalcount) + "-";
        ss +=  str(cur_time.secs) + "." + str(cur_time.nsecs);

        id.id = ss;
        id.stamp = cur_time;
        return id;
Esempio n. 21
0
 def move_robot_to_goal(self, goal, direction=1):
     id_val = "robot_{}_{}_{}".format(self.robot_id, self.goal_count,
                                      direction)
     move = MoveToPosition2DActionGoal()
     move.header.frame_id = '/robot_{}/map'.format(self.robot_id)
     goal_id = GoalID()
     goal_id.id = id_val
     move.goal_id = goal_id
     move.goal.target_pose.x = goal[0]
     move.goal.target_pose.y = goal[1]
     move.goal.target_distance = self.target_distance
     move.goal.target_angle = self.target_angle
     self.moveTo_pub.publish(move)
     self.goal_count += 1
     chosen_point = ChosenPoint()
     chosen_point.header.frame_id = '{}'.format(self.robot_id)
     chosen_point.x = goal[0]
     chosen_point.y = goal[1]
     chosen_point.direction = direction
     self.chose_point_pub.publish(chosen_point)
     self.robot_state = PASSIVE_STATE
     self.is_initial_move = False
    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. 23
0
 def move_robot_to_goal(self, goal, direction=1):
     id_val = "robot_{}_{}_{}".format(self.robot_id, self.goal_count, direction)
     move = MoveToPosition2DActionGoal()
     move.header.frame_id = '/robot_{}/map'.format(self.robot_id)
     goal_id = GoalID()
     goal_id.id = id_val
     move.goal_id = goal_id
     move.goal.header.frame_id= '/robot_{}/map'.format(self.robot_id)
     move.goal.target_pose.x = goal[0]
     move.goal.target_pose.y = goal[1]
     move.goal.target_distance = self.target_distance
     move.goal.target_angle = self.target_angle
     self.moveTo_pub.publish(move)
     self.goal_count += 1
     if self.robot_type == RR_TYPE and direction == BACK_TO_ORIGIN:
         self.navigation_start_time = rospy.Time.now()
     pu.log_msg(self.robot_id,"New goal ID: {}".format(self.goal_count),self.debug_mode)
     chosen_point = ChosenPoint()
     chosen_point.header.frame_id = '{}'.format(self.robot_id)
     chosen_point.x = goal[0]
     chosen_point.y = goal[1]
     chosen_point.direction = direction
     self.robot_stopped = False
     self.chose_point_pub.publish(chosen_point)
Esempio n. 24
0
    def cancel_current_cartesian_path_execution(self):
        cancel_goal_id = GoalID()
        cancel_goal_id.stamp = rospy.Time.now()
        cancel_goal_id.id = self.current_goal_id

        self.cancel_execution_publisher.publish(cancel_goal_id)
Esempio n. 25
0
    def resolveCMD(self):
        buffer_len = self.device.inWaiting()
        # if self.active:
        #     self.ID = 0
        #     self.content = [0] * 20

        if buffer_len >= (pack_len * 3):
            print("need to empty buffer")
            self.device.flushInput()
        elif buffer_len == 0:
            # print("time out occur!")
            # self.ID = [0]
            # self.content[-1] = 1
            # cmd_serial = [9] + self.ID + self.content
            # cs = checksum(self.header + [9] + self.ID + self.content)
            # cmdstr_start = array("B", self.header + cmd_serial + cs).tostring()
            # time.sleep(0.2)
            # self.device.write(cmdstr_start)
            return

        # read data from serial port
        header1 = ord(self.device.read(1))

        if header1 == self.header[0]:
            header2 = ord(self.device.read(1))
            if header2 == self.header[1]:
                data_len = ord(self.device.read(1))
                data = [ord(b) for b in self.device.read(data_len)]
                cs_read_high = ord(self.device.read(1))
                cs_read_low = ord(self.device.read(1))
                cs_read = cs_read_high * 256 + cs_read_low
                cs_check = checksum(data)
                print(data_len, cs_read, cs_check)
                if cs_check == cs_read:
                    print("checksum pass")

                else:
                    print("checksum wrong!")
                    self.ID = 0
                    self.content[0:data_len] = data
                    self.content[7] = 3
                    cmd_serial = [self.sendSize] + [self.ID] + self.content
                    cs = checksum([self.ID] + self.content)
                    cmdstr_start = array("B", self.header + cmd_serial +
                                         [cs]).tostring()
                    time.sleep(0.2)
                    self.device.write(cmdstr_start)
                    return
            else:
                print("header 2 wrong!")
                self.ID = 0
                self.content[7] = 2
                cmd_serial = [self.sendSize] + [self.ID] + self.content
                cs = checksum([self.ID] + self.content)
                cmdstr_start = array("B", self.header + cmd_serial +
                                     [cs]).tostring()
                time.sleep(0.2)
                self.device.write(cmdstr_start)
                return
        else:
            print("header 1 wrong!")
            self.ID = 0
            self.content[7] = 2
            cmd_serial = [self.sendSize] + [self.ID] + self.content
            cs = checksum([self.ID] + self.content)
            cmdstr_start = array("B",
                                 self.header + cmd_serial + [cs]).tostring()
            time.sleep(0.2)
            self.device.write(cmdstr_start)
            return

        # resolve data after get one full packet
        self.ID = data[0]

        if self.ID == 1:
            # TODO velocity command
            self.vx = data[1] + data[2] / 100.0
            if data[3] > 127:
                self.wz = (data[3] - 255) - (data[4] / 100.0)
            else:
                self.wz = data[3] + data[4] / 100.0
            self.dir = data[6]
            print("vx = %s, wz=%s, dir=%s" % (self.vx, self.wz, self.dir))
            rospy.loginfo("sending velocity commands")
            vel_msg = Twist()
            vx = (self.dir - 1) * self.vx
            wz = self.wz
            vel_msg.linear.x = vx
            vel_msg.angular.z = wz
            t0 = time.time()
            while (time.time() - t0) < 0.2:
                self.velPub.publish(vel_msg)
                time.sleep(0.01)
            vel_msg = Twist()
            self.velPub.publish(vel_msg)

        elif self.ID == 2:
            # TODO move goal command, stop current move base action no matter what
            self.moveStatus = 0
            goal = GoalID()
            goal.stamp = rospy.Time.now()
            goal.id = ""
            self.cancelPub.publish(goal)

            if data[1] > 127:
                self.posx = (data[1] - 255) - (data[2] / 100.0)
            else:
                self.posx = data[1] + (data[2] / 100.0)

            if data[3] > 127:
                self.posy = (data[3] - 255) - (data[4] / 100.0)
            else:
                self.posy = data[3] + (data[4] / 100.0)

            self.posz = (data[5] * 256 + data[6]) / 10000.0
            print("goal pose is %s, %s, %s" %
                  (self.posx, self.posy, self.posz))

            print("sending move goal command")
            goal_msg = PoseStamped()
            goal_msg.header.frame_id = "/map"
            goal_msg.header.stamp = rospy.Time.now()
            px, py, pz = self.posx, self.posy, self.posz
            goal_msg.pose.position.x = px
            goal_msg.pose.position.y = py
            [qw, qx, qy, qz] = eul2quat([pz, 0, 0])
            goal_msg.pose.orientation.w = qw
            goal_msg.pose.orientation.x = qx
            goal_msg.pose.orientation.y = qy
            goal_msg.pose.orientation.z = qz
            self.moveGoalPub.publish(goal_msg)

        elif self.ID == 3:
            # TODO initial pose command, should check if amcl is turned on
            if data[1] > 127:
                self.posx = (data[1] - 255) - (data[2] / 100.0)
            else:
                self.posx = data[1] + (data[2] / 100.0)

            if data[3] > 127:
                self.posy = (data[3] - 255) - (data[4] / 100.0)
            else:
                self.posy = data[3] + (data[4] / 100.0)

            self.posz = (data[5] * 256 + data[6]) / 10000.0
            print("initial pose is %s, %s, %s" %
                  (self.posx, self.posy, self.posz))
            init_msg = PoseWithCovarianceStamped()
            init_msg.header.stamp = rospy.Time.now()
            init_msg.header.frame_id = '/map'
            px, py, pz = self.posx, self.posy, self.posz
            init_msg.pose.pose.position.x = px
            init_msg.pose.pose.position.y = py
            [qw, qx, qy, qz] = eul2quat([pz, 0, 0])
            init_msg.pose.pose.orientation.w = qw
            init_msg.pose.pose.orientation.x = qx
            init_msg.pose.pose.orientation.y = qy
            init_msg.pose.pose.orientation.z = qz
            self.initialPub.publish(init_msg)

        elif self.ID == 4:
            # TODO gmapping related commands
            cmd_srv = data[7]
            arg_srv = data[8] * 256 + data[9]
            req = cmdRequest()
            req.cmd = cmd_srv
            if cmd_srv == 1:
                rospy.loginfo("start gmapping")
            elif cmd_srv == 2:
                rospy.loginfo("stop gmapping")
            elif cmd_srv == 3:
                rospy.loginfo("pause gmapping")
            elif cmd_srv == 4:
                rospy.loginfo("resume gmapping")
            else:
                rospy.logwarn("invalid command")
                return
            response = self.gmapService(req)
            self.moveStatus = response.result
            self.cmd_srv = cmd_srv
            self.arg_srv = arg_srv

        elif self.ID == 5:
            # TODO map saver related commands, map filename should base on argument value
            cmd_srv = data[7]
            arg_srv = data[8] * 256 + data[9]
            req = cmdRequest()
            req.cmd = cmd_srv
            req.str = '/home/tongsky/test'
            if cmd_srv == 1:
                rospy.loginfo("save map")
            elif cmd_srv == 2:
                rospy.loginfo("stop save map")
            else:
                rospy.logwarn("invalid command")
                return

            response = self.saverService(req)
            self.moveStatus = response.result
            self.cmd_srv = cmd_srv
            self.arg_srv = arg_srv

        elif self.ID == 6:
            # TODO map server related commands, map filename should base on argument value
            cmd_srv = data[7]
            arg_srv = data[8] * 256 + data[9]
            req = cmdRequest()
            req.cmd = cmd_srv
            req.str = '/home/tongsky/test.yaml'
            if cmd_srv == 1:
                rospy.loginfo("import map")
            else:
                rospy.logwarn("invalid command")
                return
            response = self.serverService(req)
            self.moveStatus = response.result
            self.cmd_srv = cmd_srv
            self.arg_srv = arg_srv

        elif self.ID == 7:
            # TODO map server related commands, start amcl should check map imported/ gmapping stop?
            cmd_srv = data[7]
            arg_srv = data[8] * 256 + data[9]
            req = cmdRequest()
            req.cmd = cmd_srv
            if cmd_srv == 1:
                rospy.loginfo("start amcl")
            elif cmd_srv == 2:
                rospy.loginfo("stop amcl")
            else:
                rospy.logwarn("invalid command")
                return
            response = self.amclService(req)
            self.moveStatus = response.result
            self.cmd_srv = cmd_srv
            self.arg_srv = arg_srv

        self.active = True
def katana_traject_ctrl():
    global read_joint_names
    rospy.loginfo(str(read_joint_names))

    pub = rospy.Publisher('/katana_arm_controller/joint_trajectory_action/goal', jtag, queue_size=10)

    h = std_msgs.msg.Header()
    h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work

    g = GoalID()
    g.stamp = rospy.Time.now()
    g.id = 'SimpleKatana trajectory test - ' + str(h.stamp.secs) + ' . ' + str(h.stamp.nsecs)

    jtag_msg = jtag()
    jtag_msg.header = h
    jtag_msg.goal_id = g

    # Create points
    p0 = jtp()
    p0.positions = joint_pos;
    joint_pos_original=joint_pos
    p0.time_from_start = rospy.Time.now()
    p1 = jtp()
    p1.positions = [0,2,0,0,0]
    p1.time_from_start = rospy.Time.now() + rospy.Duration(5)
    p2 = jtp()
    p2.positions = joint_pos_original
    p2.time_from_start = rospy.Time.now() + rospy.Duration(10)


    # Create the Trajectory Goal
    traject = jt()
    '''
    0: katana_motor1_pan_joint
    1: katana_motor2_lift_joint
    2: katana_motor3_lift_joint
    3: katana_motor4_lift_joint
    4: katana_motor5_wrist_roll_joint
    '''
    traject.joint_names = read_joint_names # ['katana_motor1_pan_joint','katana_motor2_lift_joint','katana_motor3_lift_joint','katana_motor4_lift_joint','katana_motor5_wrist_roll_joint']
    traject.points = [p0,p1]


    goal = jtag_goal()
    goal.trajectory=traject

    jtag_msg.goal = goal



    rate = rospy.Rate(0.05) # 10hz


    while not rospy.is_shutdown():
        p0 = jtp()
        p0.positions = joint_pos[0:5];
        joint_pos_original=joint_pos[0:5]
        p0.time_from_start = rospy.Time.now()
        p1 = jtp()
        p1.positions = [0,2,0,0,0]
        p1.time_from_start = rospy.Time.now() + rospy.Duration(8)
        p2 = jtp()
        p2.positions = joint_pos_original
        p2.time_from_start = rospy.Time.now() + rospy.Duration(16)
        traject.joint_names = read_joint_names[0:5]
        traject.points = [p0,p1,p2]
        h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
        g.stamp = rospy.Time.now()
        g.id = 'SimpleKatana trajectory test - ' + str(h.stamp.secs) + ' . ' + str(h.stamp.nsecs)
        rospy.loginfo(jtag_msg)
        pub.publish(jtag_msg)
        rate.sleep()
 def cancel(self):
     with self.moving_lock:
         if self.is_moving==1:
             goal_id=GoalID()
             goal_id.id=self.side+"_"+str(self.current_goal_id)
             self.pub_traj_cancel.publish(goal_id)
Esempio n. 28
0
    def _on_base_stop_btn_pressed(self):

        data8 = GoalID()
        data8.id = ""
        self._base_stop_pub.publish(data8)
Esempio n. 29
0
 def timer_callback(self, msg):
     stop_msg = GoalID()
     stop_msg.id = ''
     if self.stop:
         self.stop_pub.publish(stop_msg)
Esempio n. 30
0
def chat_server():

    server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    server_socket.bind((HOST, PORT))
    server_socket.listen(10)

    # add server socket object to the list of readable connections
    SOCKET_LIST.append(server_socket)

    print "Chat server started on port " + str(PORT)

    rospy.init_node("publish_string")
    pub = rospy.Publisher("publish_string", String, queue_size=10)
    stopgoal = rospy.Publisher("move_base/cancel", GoalID, queue_size=10)
    data = String()
    rate = rospy.Rate(10)
    msg = GoalID()

    while 1:

        # get the list sockets which are ready to be read through select
        # 4th arg, time_out  = 0 : poll and never block
        ready_to_read, ready_to_write, in_error = select.select(
            SOCKET_LIST, [], [], 0)

        for sock in ready_to_read:
            # a new connection request recieved
            if sock == server_socket:
                sockfd, addr = server_socket.accept()
                SOCKET_LIST.append(sockfd)
                print "Client (%s, %s) connected" % addr

                broadcast(server_socket, sockfd,
                          "[%s:%s] entered ourchatting room\n" % addr)

            # a message from a client, not a new connection
            else:
                # process data recieved from client,
                try:
                    # receiving data from the socket.
                    data = sock.recv(RECV_BUFFER)
                    if data:
                        # there is something in the socket
                        broadcast(server_socket, sock, "\r" + data)
                        myfile = open('testfile2.txt', 'w')
                        print 'opened file'
                        myfile.write(str(data))
                        myfile.close()
                        print 'finished writing file'
                        print 'publishing to ROS'
                        pub.publish(str(data))
                        print 'Publishing OK'
                        print data
                        if str(data) == '0':
                            msg.id = ''
                            print "message.id printed"
                            print msg.id
                            stopgoal.publish(msg)
                            print "stopgoal just plublished!"

                        print 'im here now'
                        #rospy.spin()
                    else:
                        # remove the socket that's broken
                        if sock in SOCKET_LIST:
                            SOCKET_LIST.remove(sock)

                        # at this stage, no data means probably theconnection has been broken
                        broadcast(server_socket, sock,
                                  "Client (%s, %s)is offline\n" % addr)

                # exception
                except:
                    broadcast(server_socket, sock,
                              "Client (%s, %s) isoffline\n" % addr)
                    continue

    server_socket.close()