Python JointState.name方法代码示例

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
def motor_pub():
        p1 = rospy.Publisher('/gBot/left_wheel/cmd', JointState)
	p2 = rospy.Publisher('/gBot/right_wheel/cmd', JointState)
       
        rospy.init_node('motorPub', anonymous=True)
 	rospy.on_shutdown(motor_stop)
        #Message for left_wheel
	msg1 = JointState()
        msg1.name = ["left_wheel"]
        msg1.position = [0.0]
        msg1.velocity = [-100]

        #Message for right_wheel
	msg2 = JointState()
        msg2.name = ["right_wheel"]
        msg2.position = [0.0]
        msg2.velocity = [-100]
	

        while not rospy.is_shutdown():

                msg1.header.stamp = rospy.Time.now()
                p1.publish(msg1)
		msg2.header.stamp = rospy.Time.now()
                p2.publish(msg2)
		rospy.loginfo('Left Motor velocity')
		rospy.loginfo(msg1.velocity)
		rospy.loginfo('Right Motor velocity')
		rospy.loginfo(msg2.velocity)
                rospy.sleep(0.1)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
    def execute(self, userdata):
        # rospy.loginfo('Executing state %s', self.__class__.__name__)
        # self.active = True
        # self.first_call = True

        # # wait for some time to read from the topic
        # # TODO: replace with a service call
        # rospy.sleep(3)
        # userdata.obj_list = self.obj_list

        # self.active = False
        # return 'succeeded'
        rospy.loginfo("Executing state %s", self.__class__.__name__)

        self.obj_list = []

        i = 1
        for view in userdata.view_list:

            rospy.loginfo("%i view: set PTU to %s", i, view)

            joint_state = JointState()
            joint_state.header.frame_id = "tessdaf"
            joint_state.name = ["pan", "tilt"]
            joint_state.position = [float(view[0]), float(view[1])]
            joint_state.velocity = [float(1.0), float(1.0)]
            joint_state.effort = [float(1.0), float(1.0)]

            self.ptu_cmd.publish(joint_state)

            # wait until PTU has finished or point cloud get screwed up
            rospy.sleep(2)

            rospy.loginfo("%i view: receive from semantic camera", i)
            self.active = True
            self.first_call = True

            # wait for some time to read once from the topic and store it onto self.pointcloud
            # TODO: replace with a service call
            rospy.sleep(2)

            self.active = False

            i = i + 1

        userdata.obj_list = self.obj_list

        # back to original position
        joint_state = JointState()
        joint_state.header.frame_id = "tessdaf"
        joint_state.name = ["pan", "tilt"]
        joint_state.position = [float(0.0), float(0.0)]
        joint_state.velocity = [float(1.0), float(1.0)]
        joint_state.effort = [float(1.0), float(1.0)]

        self.ptu_cmd.publish(joint_state)

        return "succeeded"

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
    def execute(self, goal):
        pickle.dump(goal, open("trajecoryGoalMsgVrep.data", "wb"))
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        num_points = len(trajectory_points)
        rospy.loginfo("Received %i trajectory points" % num_points)
        pprint(goal.trajectory)

        end_time = trajectory_points[-1].time_from_start.to_sec()
        control_rate = rospy.Rate(self._control_rate)
        i=num_points
        rospy.loginfo("the num of point is %i" % i )
        #self.jointTrajectoryPublisher.publish(goal.trajectory)

        for trajectory_point in trajectory_points:
            #Publish for vrepp
            jsVrep = JointState()
            jsVrep.name = copy.deepcopy(joint_names)
            jsVrep.position = trajectory_point.positions
            for j in range(len(joint_names)):
                jsVrep.name[j] = self.vrep_joint_names[joint_names[j]]
                vrep_joint_index = int(jsVrep.name[j][-1]) - 1
                print vrep_joint_index
                if vrep_joint_index == 4:
                     self.jointStatePublisherForVrep[vrep_joint_index].publish((-1*jsVrep.position[j]) + self.vrep_position_offset[vrep_joint_index])
                else:
                    self.jointStatePublisherForVrep[vrep_joint_index].publish(jsVrep.position[j] + self.vrep_position_offset[vrep_joint_index])

            #Publisher for real robot
            js = JointState()
            #rospy.loginfo("the length of js.name is %i" %len(js.name))
            js.name=copy.deepcopy(joint_names)
            for j in range(len(joint_names)):
                #rospy.loginfo("j is %i" % j)
                js.name[j] = js.name[j]+'_'+str(i)
            i=i-1
            js.position = trajectory_point.positions
            self.jointStatePublisher.publish(js)


            control_rate.sleep()
            
        start_time = rospy.get_time()
        now_from_start = rospy.get_time() - start_time
        
        last_idx = -1
        self._result.error_code = self._result.SUCCESSFUL
        self.server.set_succeeded(self._result)

        rospy.sleep(5)
        self.execution.publish("OK")

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
 def gripper_cb(self, data):
     js = JointState()
     js.header = data.header
     js.name = ["Gripper"]
     js.position = [data.current_pos]
     js.velocity = [data.velocity]
     self.gripper_pub.publish(js)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
def handstate_callback(handstate_msg):
    assert len(handstate_msg.positions) == 4
    assert len(handstate_msg.inner_links) == 3
    assert len(handstate_msg.outer_links) == 3

    jointstate_msg = JointState()
    jointstate_msg.header.stamp = handstate_msg.header.stamp
    jointstate_msg.name = [
        joint_prefix + '00',
        # Joint j01 is a mimic joint that is published by robot_state_publisher.
        joint_prefix + '01',
        joint_prefix + '11',
        joint_prefix + '21',
        # TODO: These are not currently being published. See below.
        #joint_prefix + '02',
        #joint_prefix + '12',
        #joint_prefix + '22',
    ]
    jointstate_msg.position += [ handstate_msg.positions[3] ]
    jointstate_msg.position += handstate_msg.inner_links

    # TODO: These positions look very wrong, so we'll let robot_state_publisher
    # use the mimic ratios in HERB's URDF to compute the distal joint angles. 
    #jointstate_msg.position += handstate_msg.outer_links

    jointstate_pub.publish(jointstate_msg)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
    def execute(self, goal):
        #pickle.dump(goal, open("trajecoryGoalMsg3.data", "wb"))
        joint_names = goal.trajectory.joint_names
        trajectory_points = goal.trajectory.points
        num_points = len(trajectory_points)
        rospy.loginfo("Received %i trajectory points" % num_points)
        pprint(goal.trajectory)

        end_time = trajectory_points[-1].time_from_start.to_sec()
        control_rate = rospy.Rate(self._control_rate)
        i=num_points
        rospy.loginfo("the num of point is %i" % i )
        self.jointTrajectoryPublisher.publish(goal.trajectory)

        for trajectory_point in trajectory_points:
            js = JointState()
            #rospy.loginfo("the length of js.name is %i" %len(js.name))
            js.name=copy.deepcopy(joint_names)
            for j in range(len(joint_names)):
                #rospy.loginfo("j is %i" % j)
                js.name[j] = js.name[j]+'_'+str(i)
            i=i-1
            #js.position = trajectory_point.positions
            #js.velocity=trajectory_point.velocities
            #self.jointStatePublisher.publish(js)
            control_rate.sleep()
            
        start_time = rospy.get_time()
        now_from_start = rospy.get_time() - start_time
        
        last_idx = -1
        self._result.error_code = self._result.SUCCESSFUL
        self.server.set_succeeded(self._result)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
def rc_talker():
    '''init pyxhook'''
    #Create hookmanager
    hookman = pyxhook.HookManager()
    #Define our callback to fire when a key is pressed down
    hookman.KeyDown = key_down_event
    hookman.KeyUp   = key_up_event
    #Hook the keyboard
    hookman.HookKeyboard()
    #Start our listener
    hookman.start()

    pub = rospy.Publisher('rc_sender', String, queue_size=10)
    # 2016-10-26 test urdf joint control
    # controls a file in ~/toy_code/toy_ros_space/src/rc_sim_oub/test.launch
    #roslaunch test.launch model:='$(find urdf_tutorial)/urdf/07-physics.urdf' gui:=True
    pub_2 = rospy.Publisher('test_joint', JointState, queue_size=10)
    rospy.init_node('rc_talker')
    rate = rospy.Rate(50)
    while not rospy.is_shutdown():
        #hello_str = "hello world %s" % rospy.get_time()
        pub.publish(''.join(rc_keys))

        # 2016-10-26 test urdf joint control
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        msg.name = ['base_to_left_arm', 'base_to_right_arm']
        msg.position = [-0.3*int(rc_keys[0]),0.3*int(rc_keys[1])]
        pub_2.publish(msg)
        #print rc_keys
        rate.sleep()
        if running == False:
            #Close the listener when we are done
            hookman.cancel()
            break

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
def publish_position():
    '''joint position publisher'''    
    msg = JointState()
    msg.header.stamp = rospy.Time.now()
    msg.name = JOINT_NAME_ARRAY
    msg.position = joints_pos
    pub_js.publish(msg)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
 def update(self):
     if not self.driver_status == 'DISCONNECTED':
         # Get Joint Positions
         self.current_joint_positions = self.rob.getj()
         msg = JointState()
         msg.header.stamp = rospy.get_rostime()
         msg.header.frame_id = "robot_secondary_interface_data"
         msg.name = self.JOINT_NAMES
         msg.position = self.current_joint_positions
         msg.velocity = [0]*6
         msg.effort = [0]*6
         self.joint_state_publisher.publish(msg)
         
         # Get TCP Position
         tcp_angle_axis = self.rob.getl()
         # Create Frame from XYZ and Angle Axis
         T = PyKDL.Frame()   
         axis = PyKDL.Vector(tcp_angle_axis[3],tcp_angle_axis[4],tcp_angle_axis[5])
         # Get norm and normalized axis
         angle = axis.Normalize()
         # Make frame
         T.p = PyKDL.Vector(tcp_angle_axis[0],tcp_angle_axis[1],tcp_angle_axis[2])
         T.M = PyKDL.Rotation.Rot(axis,angle)
         # Create Pose
         self.current_tcp_pose = tf_c.toMsg(T)
         self.current_tcp_frame = T
         self.broadcaster_.sendTransform(tuple(T.p),tuple(T.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
    def default(self, ci="unused"):
        """ Publish the data of the posture sensor as a ROS JointState message """
        js = JointState()
        js.header = self.get_ros_header()

        js.name = [
            "kuka_arm_0_joint",
            "kuka_arm_1_joint",
            "kuka_arm_2_joint",
            "kuka_arm_3_joint",
            "kuka_arm_4_joint",
            "kuka_arm_5_joint",
            "kuka_arm_6_joint",
            "head_pan_joint",
            "head_tilt_joint",
        ]
        js.position = [
            self.data["seg0"],
            self.data["seg1"],
            self.data["seg2"],
            self.data["seg3"],
            self.data["seg4"],
            self.data["seg5"],
            self.data["seg6"],
            self.data["pan"],
            self.data["tilt"],
        ]
        # js.velocity = [1, 1, 1, 1, 1, 1, 1]
        # js.effort = [50, 50, 50, 50, 50, 50, 50]

        self.publish(js)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
 def publishJointState(self):
     msg = JointState()
     msg.header.stamp = rospy.Time.now()
     msg.name = self.jointNames
     msg.position = self.getJointAngles()
     msg.effort = self.getLoads()
     self.jointStatePub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
def sim_robot():

    rospy.init_node("simulated_robot")
    pub = rospy.Publisher("joint_states", JointState, queue_size=5)

    rospy.Subscriber("joint_1/command", Float64, lambda msg: joint_cb(msg, 0))
    rospy.Subscriber("joint_2/command", Float64, lambda msg: joint_cb(msg, 1))
    rospy.Subscriber("joint_3/command", Float64, lambda msg: joint_cb(msg, 2))
    rospy.Subscriber("joint_4/command", Float64, lambda msg: joint_cb(msg, 3))
    rospy.Subscriber("joint_5/command", Float64, lambda msg: joint_cb(msg, 4))
    rospy.Subscriber("joint_7/command", Float64, lambda msg: joint_cb(msg, 5))

    msg = JointState()
    msg.header = Header(stamp=rospy.Time.now())
    msg.name = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_7"]
    msg.position = [0] * 6
    msg.velocity = [0, 0, 0, 0, 0, 0]
    msg.effort = [0, 0, 0, 0, 0, 0]

    def joint_cb(msg_in, id):
        msg.header = Header(stamp=rospy.Time.now())
        msg.position[id] = msg_in.data

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
 def set_stiffness(self):
     stiffness_message = JointState()
     stiffness_message.name = ["HeadYaw", "HeadPitch"]
     stiffness_message.position = [0,0]
     stiffness_message.velocity = [0,0]
     stiffness_message.effort   = [1,1]
     self.stiffness_publisher.publish(stiffness_message)

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
def resethead(): # rotate the head(not PTU)
    pub = rospy.Publisher('/head/commanded_state', JointState)       
    head_command = JointState() 
    head_command.name=["HeadPan"]
    head_command.position=[0.0] #forwards  
    pub.publish(head_command)
    eyeplay(0)# reset eyes

# 需要导入模块: from sensor_msgs.msg import JointState [as 别名]
# 或者: from sensor_msgs.msg.JointState import name [as 别名]
 def js_cb(self, a):
     rospy.loginfo('Received array')
     js = JointState()
     js.name = ['head_pan', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
     jList = a.data
     jMatrix = np.reshape(jList,(jList.shape[0]/15,15))
     i = 0
     for pos in jMatrix:
         rospy.loginfo(pos)
         js.position = pos
         gsvr = GetStateValidityRequest()
         rs = RobotState()
         rs.joint_state = js
         gsvr.robot_state = rs
         gsvr.group_name = "both_arms"
         resp = self.coll_client(gsvr)
         if (resp.valid == False):
             rospy.loginfo('false')
             rospy.loginfo(i)
             self.js_pub.publish(0)
             return
         i = i + 1
     self.js_pub.publish(1)
     rospy.loginfo('true')
     return