本文整理汇总了Python中sensor_msgs.msg.JointState方法的典型用法代码示例。如果您正苦于以下问题:Python msg.JointState方法的具体用法?Python msg.JointState怎么用?Python msg.JointState使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg
的用法示例。
在下文中一共展示了msg.JointState方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: talker_ctrl
# 需要导入模块: 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()
示例2: fk_service_client
# 需要导入模块: 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
示例3: talker_ctrl
# 需要导入模块: 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!")
示例4: thruster_ctrl_msg
# 需要导入模块: 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
示例5: talker_ctrl
# 需要导入模块: 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!")
示例6: __init__
# 需要导入模块: 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
示例7: get_endeffector_pos
# 需要导入模块: 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
示例8: start_ros_subscribers
# 需要导入模块: 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)
示例9: _callback_joint_states
# 需要导入模块: 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()
示例10: _callback_joint_states
# 需要导入模块: 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()
示例11: get_current_fk
# 需要导入模块: 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
示例12: cmd_cb
# 需要导入模块: 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
示例13: get
# 需要导入模块: 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]))
示例14: add_viapoint
# 需要导入模块: 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)
示例15: read_encoder_values
# 需要导入模块: 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