...
 
Commits (2)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, os, sys
from actionlib import SimpleActionClient
import xmlrpclib
from sweetie_bot_control_msgs.msg import FollowStepSequenceAction
from sweetie_bot_control_msgs.msg import FollowStepSequenceGoal
from sweetie_bot_control_msgs.msg import FollowStepSequenceResult
from actionlib_msgs.msg import GoalStatus
def print_help():
print 'Usage: PARAM'.format(sys.argv[0])
print '''
Execute sweetie_bot_control_msgs/FollowStepSequenceGoal stored in PARAM.
'''
sys.exit(0)
# parse command line arguments
if len(sys.argv) < 2:
print_help()
rospy.init_node('step_sequence_execute')
# get ROS parameter name
param = sys.argv[1]
param_value = rospy.get_param(param)
if not isinstance(param_value, xmlrpclib.Binary):
rospy.logerr('ROS parameter type is not Binary.')
sys.exit(-1)
# deserialize message
msg = FollowStepSequenceGoal()
msg.deserialize(param_value.data)
del msg.base_motion.points[:]
# execute message
client = SimpleActionClient('motion/controller/step_sequence', FollowStepSequenceAction)
if not client.wait_for_server(timeout=rospy.Duration(3.0)):
rospy.logerr('motion/controller/step_sequence is not available.')
sys.exit(-1)
print('Start FollowStepSequence action...')
client.send_goal_and_wait(msg)
state = client.get_state()
result = client.get_result()
# parse result
if state == GoalStatus.SUCCEEDED:
# everything is good
print('FollowStepSequence action succesed: ' + repr(result.error_code) + " " + result.error_string)
elif state in [ GoalStatus.ABORTED, GoalStatus.PREEMPTED ]:
# perhaps we stuck in midway due to tolerance error or controller switch.
print('FollowStepSequence action aborted: ' + repr(result.error_code) + " " + result.error_string)
elif state in [ GoalStatus.REJECTED, GoalStatus.RECALLED ]:
# Execution has not started, perhaps due to invalid goal.
print('FollowStepSequence action rejected: ' + repr(result.error_code) + " " + result.error_string)
else:
print('warn', 'ExecuteStepSequence: FollowStepSequence action failed (' + repr(state) + '): ' + repr(result.error_code) + " " + result.error_string)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy, os, sys
import xmlrpclib
import StringIO
from sweetie_bot_control_msgs.msg import FollowStepSequenceGoal
from geometry_msgs.msg import Pose
from kdl_msgs.msg import Twist
def print_help():
print 'Usage: PARAM'.format(sys.argv[0])
print '''
In sweetie_bot_control_msgs/FollowStepSequenceGoal base position, speed and acceleration are set to zero,
contacts indication is also removed. Resulting trajectory can be used for servo model identification in
setup where robot base is fixed and legs are in air.
'''
sys.exit(0)
# parse command line arguments
if len(sys.argv) < 2:
print_help()
# get ROS parameter name
param = sys.argv[1]
param_value = rospy.get_param(param)
if not isinstance(param_value, xmlrpclib.Binary):
raise KeyError, 'ROS parameter type is not Binary.'
# deserialize message
msg = FollowStepSequenceGoal()
msg.deserialize(param_value.data)
# remove information about base movement
for k in range(len(msg.base_motion.points)):
msg.base_motion.points[k].pose = Pose()
msg.base_motion.points[k].twist = Twist()
msg.base_motion.points[k].accel = Twist()
# remove contacts
for ee in range(len(msg.ee_motion)):
for k in range(len(msg.ee_motion[ee].points)):
msg.ee_motion[ee].points[k].contact = False
# serialize and save message
buf = StringIO.StringIO()
msg.serialize(buf)
rospy.set_param(param, xmlrpclib.Binary(buf.getvalue()))
sys.exit(0)
......@@ -121,12 +121,12 @@ compassCalCorr33=0.890816
# Accel calibration
AccelCalValid=true
AccelCalMinX=-2.013167
AccelCalMinY=-2.038227
AccelCalMinZ=-2.010424
AccelCalMaxX=2.001492
AccelCalMaxY=2.025534
AccelCalMaxZ=2.068987
AccelCalMinX=-1.985398
AccelCalMinY=-2.010113
AccelCalMinZ=-1.982693
AccelCalMaxX=1.973884
AccelCalMaxY=1.997595
AccelCalMaxZ=2.040448
# #####################################################################
#
......
Subproject commit 7acc5d85f64e048b0bb2dd28491bfb8495d09053
Subproject commit 73e2d4fe94635f05f0c87a9ccd191063d85f7a52