Ejemplo n.º 1
0
        def _BroadcastJointStateinfo_P1(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        P1 = ((321-float(lineParts[1]))*0.008)-0.15
                        P2 = self.left_tilt
                        P3 = float(lineParts[3])# current
                        P4 = 0#float(lineParts[4])# speed
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P1_MotorPublisher.publish(Motor_State)
                        self._left_tilt_Publisher.publish(P1)
                        Joint_State = JointState()
                        Joint_State.name = "left_arm_tilt_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
                        self._P1_JointPublisher.publish(Joint_State)
                        #rospy.logwarn(Joint_State)

                except:
                        rospy.logwarn("Unexpected error:left_arm_tilt_joint" + str(sys.exc_info()[0]))
    def __init__(self):
        rospy.init_node('joint_states_publisher')

        motors_list = rospy.get_param('~motors_list')
        joint_state_publisher_topic = rospy.get_param('~joint_state_topic', '/dynamixel/joint_states')
        urdf_file_path = rospy.get_param('~urdf_file_path', '')

        self.joint_state = {}
        if len(urdf_file_path) == 0:
            robot = URDF.from_parameter_server()
        else:
            robot = URDF.from_xml_string(open(urdf_file_path,'r').read())

        for joint in robot.joints:
            joint_state = JointState()
            joint_state.name = joint.name
            joint_state.current_pos = 0
            self.joint_state[joint.name] = joint_state

        for motor in motors_list:
            joint_state_topic = '/dynamixel/' + motor + '/state'
            rospy.Subscriber(joint_state_topic, JointState, self.state_callback)

        self.publisher = rospy.Publisher(joint_state_publisher_topic, SensorJointState, queue_size=1)
        self.fix_publisher = rospy.Publisher('/joint_states', SensorJointState, queue_size=1)
    
        rate = rospy.Rate(20)
        while not rospy.is_shutdown():
            self.publish_joint_states()
            rate.sleep()
Ejemplo n.º 3
0
        def _BroadcastJointStateinfo_P1(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        P1 = 0-((float(lineParts[1])* 0.00174532925)-(2.68 + self.cal_pan))# position
                        P2 = 0-((float(lineParts[2])* 0.00174532925)- (2.68 + self.cal_pan))# target
                        P3 = float(lineParts[3])# current
                        P4 = float(lineParts[4])# speed
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P2_MotorPublisher.publish(Motor_State)
                       
                        Joint_State = JointState()
                        Joint_State.name = "right_arm_pan_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
                        self._P1_JointPublisher.publish(Joint_State)
                        #rospy.logwarn(Joint_State)

                except:
                        rospy.logwarn("Unexpected error:pan" + str(sys.exc_info()[0]))
Ejemplo n.º 4
0
        def _BroadcastJointStateinfo_P4(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        P1 = 0-((float(lineParts[1])* 0.00174532925)-1.57)
                        P2 = 0-((float(lineParts[2])* 0.00174532925)-1.57)
                        P3 = float(lineParts[3])
                        P4 = float(lineParts[4])
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P4_MotorPublisher.publish(Motor_State)
                        #rospy.logwarn(Motor_State)
                        #rospy.logwarn(val)

                        Joint_State = JointState()
                        Joint_State.name = "right_arm_rotate_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()
                        self._P4_JointPublisher.publish(Joint_State)
                except:
                        rospy.logwarn("Unexpected error:4" + str(sys.exc_info()[0]))
Ejemplo n.º 5
0
    def steering_state_cb(data):

        js = JointState()
        js.name = data.name
        js.motor_ids = data.motor_ids
        js.current_pos = data.current_pos
        self.steer_state[js.motor_ids[0]] = js
Ejemplo n.º 6
0
 def publish_state(self):
     sent_state = JointState()
     sent_state.name = str(self._id)
     sent_state.goal_pos = self.last_goal_in_radians
     sent_state.velocity = int(
         np.interp(self.lastvel,
                   (self._pololu_vel_min, self._pololu_vel_max), (0, 100)))
     mutex.acquire()
     pos = float(self.micro_maestro.getPosition(self._channel))
     sent_state.is_moving = self.micro_maestro.isMoving(self._channel)
     mutex.release()
     sent_state.current_pos = self.usec2radians(pos)
     self.state_motor_pub.publish(sent_state)
     self.pub_plot_goal.publish(self.last_goal_in_radians)
     self.pub_plot_current_pos.publish(sent_state.current_pos)
     self.pub_plot_is_moving.publish(int(sent_state.is_moving))
Ejemplo n.º 7
0
    def _Broadcast_left_j2_joint(self, lineParts):
        partsCount = len(lineParts)
        #rospy.logwarn(P1)
        if (partsCount < 2):
            pass
        try:
            P1 = 0 - (radians(float(lineParts[1])) - 1.57)
            Joint_State = JointStateDY()
            Joint_State.name = "left_j2_joint"
            Joint_State.current_pos = P1
            Joint_State.header.stamp = rospy.Time.now()
            self._P2_JointPublisher.publish(Joint_State)
            #rospy.logwarn(Joint_State)

        except:
            rospy.logwarn("Unexpected error:left_j2_joint" +
                          str(sys.exc_info()[0]))
Ejemplo n.º 8
0
    def _Broadcast_Right_gripper_Joint(self, lineParts):
        partsCount = len(lineParts)
        #rospy.logwarn(partsCount)
        if (partsCount < 2):
            pass
        try:
            P1 = radians(float(lineParts[1])) + 1.57
            Joint_State = JointStateDY()
            Joint_State.name = "right_gripper_joint"
            Joint_State.current_pos = P1

            Joint_State.header.stamp = rospy.Time.now()
            self._P8_JointPublisher.publish(Joint_State)
            #rospy.logwarn(Joint_State)

        except:
            rospy.logwarn("Unexpected error:right_gripper_joint" +
                          str(sys.exc_info()[0]))
Ejemplo n.º 9
0
 def _HandleJoint_1_Command(self, data):
     """ republish position. """
     neck = (data)
     self.gear_ratio = 0.3
     new_pos = float(data.current_pos) * self.gear_ratio
     new_goal = float(data.goal_pos) * self.gear_ratio
     new_error = new_goal - new_pos
     jointstate = JointState()
     jointstate.name = ("head_tilt_mod_joint")
     jointstate.motor_ids = (neck.motor_ids)
     jointstate.motor_temps = (neck.motor_temps)
     jointstate.goal_pos = (new_goal)
     jointstate.current_pos = (new_pos)
     jointstate.velocity = (neck.velocity)
     jointstate.load = (neck.load)
     jointstate.is_moving = (neck.is_moving)
     jointstate.error = (new_error)
     #rospy.loginfo(jointstate)
     self._neckPublisher.publish(jointstate)
Ejemplo n.º 10
0
 def _HandleJoint_1_Command(self, data):
         """ republish position. """
         neck =(data)
         self.gear_ratio = 0.3
         new_pos =float (data.current_pos) * self.gear_ratio
         new_goal=float (data.goal_pos) * self.gear_ratio
         new_error = new_goal - new_pos
         jointstate = JointState()
         jointstate.name =("head_tilt_mod_joint")
         jointstate.motor_ids =(neck.motor_ids)
         jointstate.motor_temps =(neck.motor_temps)
         jointstate.goal_pos =(new_goal)
         jointstate.current_pos =(new_pos)
         jointstate.velocity =(neck.velocity)
         jointstate.load =(neck.load)
         jointstate.is_moving =(neck.is_moving)
         jointstate.error =(new_error)
         #rospy.loginfo(jointstate)
         self._neckPublisher.publish(jointstate)
Ejemplo n.º 11
0
    def _BroadcastJointStateinfo_P5(self, lineParts):
        partsCount = len(lineParts)
        #rospy.logwarn(partsCount)
        if (partsCount < 7):
            pass
        try:
            P1 = 0 - ((radians((float(lineParts[1]))) / 10) - 0.65)
            P2 = self.right_rotate  #0-((float(lineParts[2])* 0.00174532925)-1.57)
            P3 = float(lineParts[3])
            P4 = 0
            val = [P1, P2, P3, P4]
            Motor_State = MotorState()
            Motor_State.id = 11
            Motor_State.goal = P2
            Motor_State.position = P1
            Motor_State.speed = P4
            Motor_State.load = P3
            Motor_State.moving = 0
            Motor_State.timestamp = time.time()
            self.P5_MotorPublisher.publish(Motor_State)
            #rospy.logwarn(Motor_State)
            self._right_rotate_Publisher.publish(P1)

            Joint_State = JointState()
            Joint_State.name = "right_arm_rotate_joint"
            Joint_State.goal_pos = P2
            Joint_State.current_pos = P1
            Joint_State.velocity = P4
            Joint_State.load = P3
            Joint_State.error = P1 - P2
            Joint_State.is_moving = 0
            Joint_State.header.stamp = rospy.Time.now()
            self._P5_JointPublisher.publish(Joint_State)
            #rospy.logwarn(val)

        except:
            rospy.logwarn("Unexpected error:right_arm_rotate_joint" +
                          str(sys.exc_info()[0]))
Ejemplo n.º 12
0
    def _BroadcastJointStateinfo_P2(self, lineParts):
        partsCount = len(lineParts)
        #rospy.logwarn(partsCount)
        if (partsCount < 4):
            pass
        try:

            P1 = radians(float(lineParts[1]) * 0.2)
            P2 = self.right_tilt
            P3 = 0  #float(lineParts[3])# current
            P4 = 0  #float(lineParts[4])# speed
            val = [P1, P2, P3, P4]
            Motor_State = MotorState()
            Motor_State.id = 11
            Motor_State.goal = P2
            Motor_State.position = P1
            Motor_State.speed = P4
            Motor_State.load = P3
            Motor_State.moving = 0
            Motor_State.timestamp = time.time()
            self.P2_MotorPublisher.publish(Motor_State)
            self._right_tilt_Publisher.publish(P1)
            Joint_State = JointState()
            Joint_State.name = "right_arm_tilt_joint"
            Joint_State.goal_pos = P2
            Joint_State.current_pos = P1
            Joint_State.velocity = P4
            Joint_State.load = P3
            Joint_State.error = P1 - P2
            Joint_State.is_moving = 0
            Joint_State.header.stamp = rospy.Time.now(
            )  #.stamprospy.Time.from_sec(state.timestamp)
            self._P2_JointPublisher.publish(Joint_State)
            #rospy.logwarn(Joint_State)

        except:
            rospy.logwarn("Unexpected error:right_arm_tilt_joint" +
                          str(sys.exc_info()[0]))
Ejemplo n.º 13
0
    def _BroadcastJointStateinfo_P2(self, lineParts):
        partsCount = len(lineParts)
        # rospy.logwarn(partsCount)
        if partsCount < 7:
            pass
        try:
            off = 299
            A1 = float(lineParts[1]) - off
            P1 = 0 - ((A1 * 0.00174532925) - 0)
            A2 = float(lineParts[2]) - off
            P2 = 0 - ((A2 * 0.00174532925) - 0)
            P3 = float(lineParts[3])  # current
            P4 = 0  # float(lineParts[4])# speed
            val = [P1, P2, P3, P4]
            Motor_State = MotorState()
            Motor_State.id = 11
            Motor_State.goal = P2
            Motor_State.position = P1
            Motor_State.speed = P4
            Motor_State.load = P3
            Motor_State.moving = 0
            Motor_State.timestamp = time.time()
            self.P2_MotorPublisher.publish(Motor_State)
            self._right_tilt_Publisher.publish(P1)
            Joint_State = JointState()
            Joint_State.name = "right_arm_tilt_joint"
            Joint_State.goal_pos = P2
            Joint_State.current_pos = P1
            Joint_State.velocity = P4
            Joint_State.load = P3
            Joint_State.error = P1 - P2
            Joint_State.is_moving = 0
            Joint_State.header.stamp = rospy.Time.now()  # .stamprospy.Time.from_sec(state.timestamp)
            self._P2_JointPublisher.publish(Joint_State)
            # rospy.logwarn(Joint_State)

        except:
            rospy.logwarn("Unexpected error:right_arm_tilt_joint" + str(sys.exc_info()[0]))
Ejemplo n.º 14
0
        def _BroadcastJointStateinfo_P2(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        cal = 0
                        A1 = (1023 + cal) - float(lineParts[1])
                        P1 = (A1 * 0.003070961)#(3.8-((float(lineParts[1])* 0.004597701)))# position
                        A2 = (1023 + cal) - float(lineParts[1])
                        P2 = (A2* 0.003070961)#float(lineParts[2])# target
                        P3 = float(lineParts[3])# current
                        P4 = float(lineParts[4])# speed
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P2_MotorPublisher.publish(Motor_State)
                       
                        Joint_State = JointState()
                        Joint_State.name = "right_arm_lift_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()#.stamprospy.Time.from_sec(state.timestamp)
                        self._P2_JointPublisher.publish(Joint_State)
                        #rospy.logwarn(Joint_State)

                except:
                        rospy.logwarn("Unexpected error:2" + str(sys.exc_info()[0]))
Ejemplo n.º 15
0
        def _BroadcastJointStateinfo_P3(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(partsCount)
                if (partsCount  < 7):
                        pass
                try:
                        #offset = Float(-1.57)
                        P1 = (float(lineParts[1])/1000)-0.6
                        P2 = self.right_lift #0-((float(lineParts[2])* 0.00174532925)-1.57)
                        P3 = 0#float(lineParts[3])
                        P4 = 0
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P3_MotorPublisher.publish(Motor_State)
                        #rospy.logwarn(Motor_State)
                        self._left_lift_Publisher.publish(P1)

                        Joint_State = JointState()
                        Joint_State.name = "left_arm_lift_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()
                        self._P3_JointPublisher.publish(Joint_State)

                except:
                        rospy.logwarn("Unexpected error:left_arm_lift_joint" + str(sys.exc_info()[0]))
Ejemplo n.º 16
0
        def _BroadcastJointStateinfo_P8(self, lineParts):
                partsCount = len(lineParts)
                #rospy.logwarn(lineParts)
                if (partsCount  < 4):
                        pass
                try:
                        P1 = radians(float(lineParts[1]))#P1 = radians(float(lineParts[1]))
                        P2 = self.pan
                        P3 = 0#float(lineParts[3])
                        P4 = 0
                        val = [P1, P2, P3, P4]
                        Motor_State = MotorState()
                        Motor_State.id = 11
                        Motor_State.goal = P2
                        Motor_State.position = P1
                        Motor_State.speed = P4
                        Motor_State.load = P3
                        Motor_State.moving = 0
                        Motor_State.timestamp = time.time()
                        self.P8_MotorPublisher.publish(Motor_State)
                        self._pan_Publisher.publish(P1)

                        Joint_State = JointState()
                        Joint_State.name = "pan_joint"
                        Joint_State.goal_pos = P2
                        Joint_State.current_pos = P1
                        Joint_State.velocity = P4
                        Joint_State.load = P3
                        Joint_State.error = P1 - P2
                        Joint_State.is_moving = 0
                        Joint_State.header.stamp = rospy.Time.now()
                        self._P8_JointPublisher.publish(Joint_State)
                        #rospy.logwarn(val)

                except:
                        pass#rospy.logwarn("Unexpected error:pan_joint" + str(sys.exc_info()[0]))