class MICOMockup(object):
"""
Subscribe
JointVelocity(mico_jointvel_input_topic)
Publish
JointState(mico_jointstate_output_topic)
JointAngles(mico_jointangles_output_topic)
"""
def __init__(self):
self._joint_ang = joint_angles_deg2rad(JointAngles(joint1=0.0, joint2=-90.0, joint3=0.0,
joint4=0.0, joint5=90.0, joint6=0.0))
self._joint_vel = JointVelocity(joint1=0.0, joint2=0.0, joint3=0.0,
joint4=0.0, joint5=0.0, joint6=0.0)
self._joint_state = JointState(name=['mico_joint_' + suffix for suffix in ['1', '2', '3', '4', '5', '6',
'finger_1', 'finger_2']],
position=[getattr(self._joint_ang, a) for a in JointAngles.__slots__] + [0, 0],
velocity=[getattr(self._joint_vel, a) for a in JointVelocity.__slots__] + [0, 0])
self._joint_state_publisher = rospy.Publisher(rospy.get_param(PARAM_NAME_MICO_JOINTSTATE_OUTPUT_TOPIC),
JointState, queue_size=1)
self.timer = LoopTimeManager()
def activate(self):
self._joint_state_publisher.publish()
rospy.Subscriber(rospy.get_param(PARAM_NAME_MICO_JOINTVEL_INPUT_TOPIC),
JointVelocity, self._joint_vel_callback)
rospy.loginfo('mico_mock activated')
return self
# HACKED
def _joint_vel_callback(self, joint_vel):
dt = self.timer.dt_sec
for attr in JointAngles.__slots__:
j_ang, j_vel = getattr(self._joint_ang, attr), getattr(self._joint_vel, attr)
setattr(self._joint_ang, attr, j_ang + j_vel * dt)
self._joint_vel = joint_vel
self.timer.update()
def publish(self):
for idx, attr in enumerate(JointAngles.__slots__):
self._joint_state.position[idx] = getattr(self._joint_ang, attr)
for idx, attr in enumerate(JointVelocity.__slots__):
self._joint_state.velocity[idx] = getattr(self._joint_vel, attr)
self._joint_state_publisher.publish(self._joint_state)
def __init__(self):
self._joint_ang = joint_angles_deg2rad(JointAngles(joint1=0.0, joint2=-90.0, joint3=0.0,
joint4=0.0, joint5=90.0, joint6=0.0))
self._joint_vel = JointVelocity(joint1=0.0, joint2=0.0, joint3=0.0,
joint4=0.0, joint5=0.0, joint6=0.0)
self._joint_state = JointState(name=['mico_joint_' + suffix for suffix in ['1', '2', '3', '4', '5', '6',
'finger_1', 'finger_2']],
position=[getattr(self._joint_ang, a) for a in JointAngles.__slots__] + [0, 0],
velocity=[getattr(self._joint_vel, a) for a in JointVelocity.__slots__] + [0, 0])
self._joint_state_publisher = rospy.Publisher(rospy.get_param(PARAM_NAME_MICO_JOINTSTATE_OUTPUT_TOPIC),
JointState, queue_size=1)
self.timer = LoopTimeManager()
def __init__(self):
self._tf_broadcaster = tf2_ros.TransformBroadcaster()
self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
self._localized_point_tf_from_localized_origin = init_tf_stamped(
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID),
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_FRAME_ID))
self._mobile_base_tf_for_calc_from_localized_origin = init_tf_stamped(
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID_FOR_CALC),
rospy.get_param(PARAM_NAME_MOBILE_BASE_FRAME_ID_FOR_CALC))
self._localized_point_tf_for_calc_from_mobile_base = init_tf_stamped(
rospy.get_param(PARAM_NAME_MOBILE_BASE_FRAME_ID_FOR_CALC),
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_FRAME_ID_FOR_CALC))
self._input = TwistStamped()
self.timer = LoopTimeManager()
class SLAMMockup(object):
"""
Subscribe
TwistStamped(blackship_input)
tf(mobile_base_frame_id_for_calc -> localized_point_frame_id_for_calc)
Publish
tf(localized_point_origin -> localized_point_frame_id)
"""
def __init__(self):
self._tf_broadcaster = tf2_ros.TransformBroadcaster()
self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
self._localized_point_tf_from_localized_origin = init_tf_stamped(
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID),
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_FRAME_ID))
self._mobile_base_tf_for_calc_from_localized_origin = init_tf_stamped(
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID_FOR_CALC),
rospy.get_param(PARAM_NAME_MOBILE_BASE_FRAME_ID_FOR_CALC))
self._localized_point_tf_for_calc_from_mobile_base = init_tf_stamped(
rospy.get_param(PARAM_NAME_MOBILE_BASE_FRAME_ID_FOR_CALC),
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_FRAME_ID_FOR_CALC))
self._input = TwistStamped()
self.timer = LoopTimeManager()
def activate(self):
rospy.Subscriber(rospy.get_param(PARAM_NAME_BS_INPUT_TOPIC),
TwistStamped, self._bs_input_callback)
origin_to_localized_origin = wait_for_tf(self._tf_buffer,
rospy.get_param(PARAM_NAME_SYS_ORIGIN_FRAME_ID),
rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID))
origin_to_localized_origin.child_frame_id = rospy.get_param(PARAM_NAME_LOCALIZED_POINT_ORIGIN_FRAME_ID_FOR_CALC)
rospy.Publisher('/tf_static', TFMessage,
queue_size=100, latch=True).publish([origin_to_localized_origin])
self._localized_point_tf_for_calc_from_mobile_base = wait_for_tf(self._tf_buffer,
self._localized_point_tf_for_calc_from_mobile_base.header.frame_id,
self._localized_point_tf_for_calc_from_mobile_base.child_frame_id,
interval_sec=5.0)
rospy.loginfo('slam_mock activated')
return self
# 厳密にはこの更新はループ的によろしくない．入ってきたvelで今まで動いていました，となっているので．
# ↑直した．けどあまりいい方法とはいえない．．．
def _bs_input_callback(self, bs_input):
dt = self.timer.dt_sec
vdt = self._input.twist.linear.x * dt
wdt = self._input.twist.angular.z * dt
theta = euler_from_rosquaternion(self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation)[0]
translation = add_vectors([self._mobile_base_tf_for_calc_from_localized_origin.transform.translation,
Vector3(x=vdt * np.cos(theta), y=vdt * np.sin(theta))])
rotation = multiply_ros_quaternion([self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation,
Quaternion(z=np.sin(wdt / 2.0), w=np.cos(wdt / 2.0))])
self._mobile_base_tf_for_calc_from_localized_origin.header.stamp = rospy.Time.now()
self._mobile_base_tf_for_calc_from_localized_origin.transform.translation = translation
self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation = rotation
self._localized_point_tf_for_calc_from_mobile_base = update_tf(self._tf_buffer,
self._localized_point_tf_for_calc_from_mobile_base)
self._input = bs_input
self.timer.update()
def publish(self):
stamp = self._mobile_base_tf_for_calc_from_localized_origin.header.stamp
translation = add_vectors([self._mobile_base_tf_for_calc_from_localized_origin.transform.translation,
self._localized_point_tf_for_calc_from_mobile_base.transform.translation])
rotation = multiply_ros_quaternion([self._mobile_base_tf_for_calc_from_localized_origin.transform.rotation,
self._localized_point_tf_for_calc_from_mobile_base.transform.rotation])
self._localized_point_tf_from_localized_origin.header.stamp = rospy.Time.now()
self._localized_point_tf_from_localized_origin.transform.translation = translation
self._localized_point_tf_from_localized_origin.transform.rotation = rotation
self._tf_broadcaster.sendTransform([self._localized_point_tf_from_localized_origin,
self._mobile_base_tf_for_calc_from_localized_origin])
