Python msg.JointState方法代碼示例

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def talker_ctrl():
    global rate_value
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("usv_position_setpoint", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        pub_motor.publish(thruster_ctrl_msg())
        pub_rudder.publish(rudder_ctrl_msg())
        rate.sleep() 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def fk_service_client(limb = "right"):
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/FKService"
    fksvc = rospy.ServiceProxy(ns, SolvePositionFK)
    fkreq = SolvePositionFKRequest()
    joints = JointState()
    joints.name = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
                   'right_j4', 'right_j5', 'right_j6']
    joints.position = [0.763331, 0.415979, -1.728629, 1.482985,
                       -1.135621, -1.674347, -0.496337]
    # Add desired pose for forward kinematics
    fkreq.configuration.append(joints)
    # Request forward kinematics from base to "right_hand" link
    fkreq.tip_names.append('right_hand')

    try:
        rospy.wait_for_service(ns, 5.0)
        resp = fksvc(fkreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return False

    # Check if result valid 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def thruster_ctrl_msg():
    global actuator_vel
    global left
    global right
    global Kp
    msg = JointState()
    msg.header = Header()
    msg.name = ['fwd_left', 'fwd_right']
    aux = rudder_ctrl()

    left = aux + 100.
    right = -aux + 100.
    if (left < 0):
        left = 0
    if (right < 0):
        right  = 0  

    msg.position = [left, right]
    msg.velocity = []
    msg.effort = []
    return msg 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def talker_ctrl():
    global rate_value
    global result
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:    
            pub_motor.publish(thruster_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'):
        super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached)
        self._redist_rate = rospy.Rate(50)

        self._arbotix = ArbotiX('/dev/ttyUSB0')
        assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
        assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"

        self._joint_lock = Lock()
        self._angles, self._velocities = {}, {}
        rospy.Subscriber("/joint_states", JointState, self._joint_callback)
        time.sleep(1)        

        self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]]
        self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1)

        p.connect(p.DIRECT)
        widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf'
        self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) 
        p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi]))       
        self._n_errors = 0 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def get_endeffector_pos(self):
        fkreq = SolvePositionFKRequest()
        joints = JointState()
        joints.name = self._limb_right.joint_names()
        joints.position = [self._limb_right.joint_angle(j)
                           for j in joints.name]

        # Add desired pose for forward kinematics
        fkreq.configuration.append(joints)
        fkreq.tip_names.append('right_hand')
        try:
            rospy.wait_for_service(self.name_of_service, 5)
            resp = self.fksvc(fkreq)
        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("Service call failed: %s" % (e,))
            return False 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def start_ros_subscribers(self):
        self.joint_state_sub = rospy.Subscriber('/joint_states', JointState, self.sub_joint_states)
        self.robot_state_sub = rospy.Subscriber('/niryo_one/robot_state', RobotState, self.sub_robot_state)
        self.selected_tool_id_sub = rospy.Subscriber('/niryo_one/current_tool_id', Int32, self.sub_selected_tool_id)
        self.learning_mode_sub = rospy.Subscriber('/niryo_one/learning_mode', Bool, self.sub_learning_mode)
        self.joystick_enabled_sub = rospy.Subscriber('/niryo_one/joystick_interface/is_enabled', Bool,
                                                     self.sub_joystick_enabled)
        self.hardware_status_sub = rospy.Subscriber('/niryo_one/hardware_status', HardwareStatus,
                                                    self.sub_hardware_status)
        self.ros_log_status_sub = rospy.Subscriber('/niryo_one/rpi/ros_log_status', LogStatus, self.sub_ros_log_status)
        self.software_version_sub = rospy.Subscriber('/niryo_one/software_version', SoftwareVersion,
                                                     self.sub_software_version)

        rospy.Subscriber('/niryo_one/kits/conveyor_1_feedback', ConveyorFeedback,
                         self.sub_conveyor_1_feedback)
        rospy.Subscriber('/niryo_one/kits/conveyor_2_feedback', ConveyorFeedback,
                         self.sub_conveyor_2_feedback) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def _callback_joint_states(self, msg):
        """
        ROS subscriber callback for gripper joint state (position, velocity)

        :param msg: Contains message published in topic
        :type msg: sensor_msgs/JointState
        """
        self.joint_state_lock.acquire()
        for idx, name in enumerate(msg.name):
            if name in self.gripper_joint_names:
                if idx < len(msg.position):
                    self._joint_angles[name] = msg.position[idx]
                if idx < len(msg.velocity):
                    self._joint_velocities[name] = msg.velocity[idx]
                if idx < len(msg.effort):
                    self._joint_efforts[name] = msg.effort[idx]
        self.joint_state_lock.release() 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def _callback_joint_states(self, msg):
        """
        ROS subscriber callback for arm joint state (position, velocity)

        :param msg: Contains message published in topic
        :type msg: sensor_msgs/JointState
        """
        self.joint_state_lock.acquire()
        for idx, name in enumerate(msg.name):
            if name in self.arm_joint_names:
                if idx < len(msg.position):
                    self._joint_angles[name] = msg.position[idx]
                if idx < len(msg.velocity):
                    self._joint_velocities[name] = msg.velocity[idx]
                if idx < len(msg.effort):
                    self._joint_efforts[name] = msg.effort[idx]
        self.joint_state_lock.release() 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def get_current_fk(self, fk_link_names, frame_id='base_link'):
        """
        Get the current forward kinematics of a set of links.

        :param fk_link_names: [string]
                        list of links that we want to
                        get the forward kinematics from
        :param frame_id: string
                        the reference frame to be used
        :return fk_result: moveit_msgs.srv.GetPositionFKResponse
        """
        # Subscribe to a joint_states
        js = rospy.wait_for_message('/robot/joint_states', JointState)
        # Call FK service
        fk_result = self.get_fk(fk_link_names, js.name, js.position, frame_id)
        return fk_result 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def cmd_cb(self, msg):
        '''
        Callback to update the world. Listens to JointState messages so we can get
        the time.

        Parameters:
        ----------
        msg: message received from callback -- should be a joint state
        '''
        if not isinstance(msg, JointState):
            raise RuntimeError('must send a joint state')
        elif len(msg.name) is not len(msg.position):
            raise RuntimeError('number of positions and joint names must match')

        for name, pos in zip(msg.name, msg.position):
            if not name in self.qs:
                raise RuntimeError('world sent joint that does not exist')
            self.qs[name] = pos 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def get(self, x_des, seed=None, bounds=()):
        """
        Get the IK by minimization
        :param x_des: desired task space pose [[x, y, z], [x, y, z, w]]
        :param seed: RobotState message
        :param bounds:[(min, max), (min, max), (min, max), ... for each joint]
        :return: (bool, joints)
        """
        if isinstance(seed, RobotState):
            seed = seed.joint_state
        elif not isinstance(seed, JointState) and seed is not None:
            raise TypeError('ros.IK.get only accepts RS or JS, got {}'.format(type(seed)))

        seed = [seed.position[seed.name.index(joint)] for joint in self._ik.joints] if seed is not None else ()
        result = self._ik.get(x_des, seed, bounds)
        return result[0], JointState(name=self._ik.joints, position=list(result[1])) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def add_viapoint(self, t, obsys, sigmay=1e-6):
        """
        Add a viapoint i.e. an observation at a specific time
        :param t: Time of observation
        :param obsys: RobotState observed at the time
        :param sigmay:
        :return:
        """
        if isinstance(obsys, RobotState):
            obsys = obsys.joint_state
        elif not isinstance(obsys, JointState):
            raise TypeError("ros.ProMP.add_viapoint only accepts RS or JS, got {}".format(type(obsys)))
        try:
            positions = [obsys.position[obsys.name.index(joint)] for joint in self.joint_names]  # Make sure joints are in right order
        except KeyError as e:
            raise KeyError("Joint {} provided as viapoint is unknown to the demonstrations".format(e))
        else:
            self.promp.add_viapoint(t, map(float, positions), sigmay) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import JointState [as 別名]
def read_encoder_values(self):
        """Query roboclaws and update current motors status in encoder ticks"""
        enc_msg = JointState()
        enc_msg.header.stamp = rospy.Time.now()
        for motor_name, properties in self.roboclaw_mapping.iteritems():
            enc_msg.name.append(motor_name)
            position = self.read_encoder_position(properties["address"], properties["channel"])
            velocity = self.read_encoder_velocity(properties["address"], properties["channel"])
            current = self.read_encoder_current(properties["address"], properties["channel"])
            enc_msg.position.append(self.tick2position(position,
                                                       self.encoder_limits[motor_name][0],
                                                       self.encoder_limits[motor_name][1],
                                                       properties['ticks_per_rev'],
                                                       properties['gear_ratio']))
            enc_msg.velocity.append(self.qpps2velocity(velocity,
                                                       properties['ticks_per_rev'],
                                                       properties['gear_ratio']))
            enc_msg.effort.append(current)

        self.current_enc_vals = enc_msg