def load_joint_state(self, pose_ns, pose_param):
# derive parameter full name
if pose_ns:
pose_param = pose_ns + '/' + pose_param
# Load JointState message from Parameter Server
try:
goal_raw = rospy.get_param(pose_param)
except KeyError as e:
raise KeyError, "CheckJointState: Unable to get '" + pose_param + "' parameter."
if not isinstance(goal_raw, xmlrpclib.Binary):
raise TypeError, "CheckJointState: ROS parameter '" + pose_param + "' is not a binary data."
# deserialize
msg = JointState()
msg.deserialize(goal_raw.data)
# create and return joint index to simplify tolerance check
return { name: position for name, position in izip(msg.name, msg.position) }
data=''
begin=time.time()
while 1:
#if you got some data, then timeout break
if total_data and time.time()-begin>timeout:
break
#if you got no data at all, wait a little longer
elif time.time()-begin>timeout*2:
break
try:
data=self.labview_sock.recv(8192)
if data:
total_data.append(data)
begin=time.time()
except:
pass
return ''.join(total_data)
if __name__ == '__main__':
rospy.init_node('labview_server')
server = LabviewServer()
while not rospy.is_shutdown():
msg_ros = JointState()
data = server.recv_timeout()
if data:
#~ fmt = '<IiiI5sIIssIssIssIssIssIssIssssI7d'
#~ msg_py = struct.unpack(fmt, data[:struct.calcsize(fmt)])
#~ print msg_py
print msg_ros.deserialize(data)
class SetJointStateBase(Dummy):
'''
Base class for states which move robot to named pose using FollowJointState controller.
Pose is loaded from binary parameter from Parameter Server as JointState message.
Then state activate FollowJointState controller and publish pose.
Movement is considered finished when position error is less then given tolerance.
-- controller string FollowJointState controller namespace.
-- tolerance float Position tolerance (rad).
-- timeout float Movement timeout (s).
-- joint_topic string Topic where actual pose published.
<= done Finished.
<= failed Failed to activate FollowJointState controller.
<= timeout Timeout reached.
'''
def __init__(self,
controller='motion/controller/joint_state_head',
tolerance=0.17,
timeout=10.0,
joint_topic="joint_states",
outcomes=['done', 'failed', 'timeout']):
super(SetJointStateBase, self).__init__(outcomes=outcomes)
# Store topic parameter for later use.
self._controller = controller
self._joint_topic = joint_topic
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# create proxies
self._action_client = ProxyActionClient(
{self._controller: SetOperationalAction})
self._pose_publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
self._pose_subscriber = ProxySubscriberCached(
{self._joint_topic: JointState})
# timestamp
self._timestamp = None
# error in enter hook
self._error = False
def load_joint_state_msg(self, pose_ns, pose_param):
# derive parameter full name
if pose_ns:
pose_param = pose_ns + '/' + pose_param
# Load JointState message from Parameter Server
try:
goal_raw = rospy.get_param(pose_param)
except KeyError as e:
raise KeyError, "SetJointStateBase: Unable to get '" + pose_param + "' parameter."
if not isinstance(goal_raw, xmlrpclib.Binary):
raise TypeError, "SetJointStateBase: ROS parameter '" + pose_param + "' is not a binary data."
# deserialize
self._target_joint_state = JointState()
self._target_joint_state.deserialize(goal_raw.data)
# create joint index to simplify tolerance check
self._joint_target_pose = {
name: position
for name, position in izip(self._target_joint_state.name,
self._target_joint_state.position)
}
def on_enter(self, userdata):
self._error = False
# activate controller
actiavtion_request = SetOperationalGoal()
actiavtion_request.operational = True
actiavtion_request.resources = self._target_joint_state.name
try:
self._action_client.send_goal(self._controller, actiavtion_request)
except Exception as e:
Logger.logwarn(
'SetJointStateBase: Failed to send the SetOperational command:\n%s'
% str(e))
self._error = True
return
# set start timestamp
self._timestamp = Time.now()
def execute(self, userdata):
# error in start hook
if self._error:
return 'failed'
# check if controller is active
if not self._action_client.is_active(self._controller):
Logger.loginfo(
'SetJointStateBase: controller was deactivated by external cause.'
)
return 'failed'
# check if time elasped
if Time.now() - self._timestamp > self._timeout:
Logger.loginfo('SetJointStateBase: timeout.')
return 'timeout'
# publish goal pose
self._pose_publisher.publish(self._controller + '/in_joints_ref',
self._target_joint_state)
# check tolerance
joints_msg = self._pose_subscriber.get_last_msg(self._joint_topic)
on_position = True
for name, pos in izip(joints_msg.name, joints_msg.position):
target_pos = self._joint_target_pose.get(name)
if (target_pos != None):
if abs(target_pos - pos) > self._tolerance:
on_position = False
break
if on_position:
Logger.loginfo('SetJointStateBase: on position')
return 'done'
def on_exit(self, userdata):
if self._action_client.is_active(self._controller):
try:
self._action_client.cancel(self._controller)
except Exception as e:
Logger.logwarn('SetJointStateBase: failed to deactivate `' +
self._controller + '` controller:\n%s' % str(e))
