Smatch 最基础的应用
Smatch 最基础的应用
文章目录
- remapping 参数
- 示例 ： state machine 加入了 状态机的输入输出
- 只用moveit控制机械臂抓取
remapping 参数
sm_top = smach.StateMachine(outcomes=['outcome4','outcome5'],
input_keys=['sm_input'],
output_keys=['sm_output'])
with sm_top:
smach.StateMachine.add('FOO', Foo(),
# transimation 把输出和下一个状态连接
transitions={'outcome1':'BAR',
'outcome2':'outcome4'},
# remapping 把输入输出和状态机的输入输出连接起来：把状态机的输入，映射到状态的输入
remapping={'foo_input':'sm_input',
'foo_output':'sm_data'})
smach.StateMachine.add('BAR', Bar(),
transitions={'outcome2':'FOO'},
remapping={'bar_input':'sm_data',
'bar_output1':'sm_output'})
示例 ： state machine 加入了 状态机的输入输出
#!/usr/bin/env python
import rospy
import smach
import smach_ros
import time
class Pick(smach.State):
def __init__(self):
smach.State.__init__(self,outcomes=['Y','N'],
input_keys=['pick_in'],
output_keys=['pick_out'])
def execute(self,userdata):
rospy.loginfo('Executing state Pick')
# time.sleep(4)
if userdata.pick_in<3:
userdata.pick_out=userdata.pick_in+1
return 'N'
else:
return 'Y'
class Bar(smach.State):
def __init__(self):
smach.State.__init__(self,outcomes=['Y'],input_keys=['bar_in'])
def execute(self,userdata):
rospy.loginfo('Executing state BAr')
rospy.loginfo('Counter=%f'%userdata.bar_in)
time.sleep(1)
return 'Y'
rospy.init_node('smach_example_state_machine')
sm=smach.StateMachine(outcomes=['stop'])
sm.userdata.sm_counter=0
with sm:
smach.StateMachine.add('Pick',Pick(),
transitions={'N':'Bar','Y':'stop'},
remapping={'pick_in':'sm_counter',
'pick_out':'sm_counter'})
smach.StateMachine.add('Bar',Bar(),transitions={'Y':'Pick'},remapping={'bar_in':'sm_counter'})
outcome=sm.execute()
print(outcome)
rospy.loginfo('Counter=%f'%sm.userdata.sm_counter)
只用moveit控制机械臂抓取
#!/usr/bin/env python
import rospy
from math import pow, atan2, sqrt
from tf.transformations import *
import smach
import smach_ros
from smach_ros import SimpleActionState
from smach_ros import ServiceState
import threading
import time
# Navigation
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
# Manipulator
from geometry_msgs.msg import Pose
from open_manipulator_msgs.msg import JointPosition
from open_manipulator_msgs.msg import KinematicsPose
from open_manipulator_msgs.srv import SetJointPosition
from open_manipulator_msgs.srv import SetKinematicsPose
# AR Markers
from ar_track_alvar_msgs.msg import AlvarMarker
from ar_track_alvar_msgs.msg import AlvarMarkers
class getPoseOfTheObject(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'aborted'],
output_keys=['output_object_pose'])
self.namespace = "om_with_tb3"
self.marker_pose_sub = rospy.Subscriber(self.namespace + '/ar_pose_marker', AlvarMarkers, self.arMarkerMsgCallback)
self.OFFSET_FOR_GOAL_HEIGHT = 0.150
def arMarkerMsgCallback(self, ar_marker_pose_msg):
if len(ar_marker_pose_msg.markers) == 0:
self.ar_marker_pose = False
else:
self.ar_marker_pose = AlvarMarker()
self.ar_marker_pose = ar_marker_pose_msg.markers[0]
def execute(self, userdata):
if self.ar_marker_pose == False:
rospy.logwarn('Failed to get pose of the marker')
return 'aborted'
else:
object_pose = Pose()
object_pose.position = self.ar_marker_pose.pose.pose.position
object_pose.position.x += 0.0
object_pose.position.y = 0.0
object_pose.position.z += self.OFFSET_FOR_GOAL_HEIGHT
dist = math.sqrt((self.ar_marker_pose.pose.pose.position.x * self.ar_marker_pose.pose.pose.position.x) +
(self.ar_marker_pose.pose.pose.position.y * self.ar_marker_pose.pose.pose.position.y))
if self.ar_marker_pose.pose.pose.position.y > 0:
yaw = math.acos(self.ar_marker_pose.pose.pose.position.x / dist)
else:
yaw = (-1) * math.acos(self.ar_marker_pose.pose.pose.position.x / dist)
roll = 0.0
pitch = 0.0
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
object_pose.orientation.w = cy * cr * cp + sy * sr * sp
object_pose.orientation.x = cy * sr * cp - sy * cr * sp
object_pose.orientation.y = cy * cr * sp + sy * sr * cp
object_pose.orientation.z = sy * cr * cp - cy * sr * sp
userdata.output_object_pose = object_pose
rospy.loginfo('Succeeded to get pose of the object')
return 'succeeded'
self.marker_pose_sub
class getPoseOfTheBox(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'aborted'],
output_keys=['output_object_pose'])
self.namespace = "om_with_tb3"
self.marker_pose_sub = rospy.Subscriber(self.namespace + '/ar_pose_marker', AlvarMarkers, self.arMarkerMsgCallback)
self.OFFSET_FOR_STRETCH = 0.070
self.OFFSET_FOR_GOAL_HEIGHT = 0.200
def arMarkerMsgCallback(self, ar_marker_pose_msg):
if len(ar_marker_pose_msg.markers) == 0:
self.ar_marker_pose = False
else:
self.ar_marker_pose = AlvarMarker()
self.ar_marker_pose = ar_marker_pose_msg.markers[0]
def execute(self, userdata):
if self.ar_marker_pose == False:
rospy.logwarn('Failed to get pose of the marker')
return 'aborted'
else:
object_pose = Pose()
object_pose.position = self.ar_marker_pose.pose.pose.position
object_pose.position.x += self.OFFSET_FOR_STRETCH
object_pose.position.y = 0.0
object_pose.position.z += self.OFFSET_FOR_GOAL_HEIGHT
dist = math.sqrt((self.ar_marker_pose.pose.pose.position.x * self.ar_marker_pose.pose.pose.position.x) +
(self.ar_marker_pose.pose.pose.position.y * self.ar_marker_pose.pose.pose.position.y))
if self.ar_marker_pose.pose.pose.position.y > 0:
yaw = math.acos(self.ar_marker_pose.pose.pose.position.x / dist)
else:
yaw = (-1) * math.acos(self.ar_marker_pose.pose.pose.position.x / dist)
roll = 0.0
pitch = 0.0
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
object_pose.orientation.w = cy * cr * cp + sy * sr * sp
object_pose.orientation.x = cy * sr * cp - sy * cr * sp
object_pose.orientation.y = cy * cr * sp + sy * sr * cp
object_pose.orientation.z = sy * cr * cp - cy * sr * sp
userdata.output_object_pose = object_pose
rospy.loginfo('Succeeded to get pose of the object')
return 'succeeded'
self.marker_pose_sub
class getCloserToGoal(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'failed'])
self.namespace = "om_with_tb3"
self.marker_pose_sub = rospy.Subscriber(self.namespace + '/ar_pose_marker', AlvarMarkers, self.arMarkerMsgCallback)
self.odom_sub = rospy.Subscriber(self.namespace + '/odom', Odometry, self.odomMsgCallback)
self.cmd_vel_pub = rospy.Publisher(self.namespace + '/cmd_vel', Twist, queue_size=10)
self.tb3_odom = Odometry()
self.cmd_vel = Twist()
self.priv_dist = 0.0
self.priv_heading = 0.0
def arMarkerMsgCallback(self, ar_marker_pose_msg):
if len(ar_marker_pose_msg.markers) == 0:
self.ar_marker_pose = False
else:
self.ar_marker_pose = AlvarMarker()
self.ar_marker_pose = ar_marker_pose_msg.markers[0]
def odomMsgCallback(self, odom_msg):
self.tb3_odom = odom_msg
def getDistanceFromRobot(self, goal):
return goal.pose.pose.position.x
def getAngleBtwRobotAndMarker(self, goal):
return math.atan2(goal.pose.pose.position.y, goal.pose.pose.position.x)
def execute(self, userdata):
while 1:
# rospy.loginfo('ar_marker_pose.x : %f', self.ar_marker_pose.pose.pose.position.x)
# rospy.loginfo('ar_marker_pose.y : %f', self.ar_marker_pose.pose.pose.position.y)
# rospy.loginfo('ar_marker_pose.z : %f', self.ar_marker_pose.pose.pose.position.z)
# rospy.loginfo('ar_marker_pose.yaw : %f', math.degrees(self.getAngleBtwRobotAndMarker(self.ar_marker_pose)))
if self.ar_marker_pose == False:
rospy.loginfo('Failed to get pose of the marker')
self.cmd_vel.linear.x = -0.04
self.cmd_vel.angular.z = 0.0
self.cmd_vel_pub.publish(self.cmd_vel)
continue
dist = self.getDistanceFromRobot(self.ar_marker_pose) # meter
heading = self.getAngleBtwRobotAndMarker(self.ar_marker_pose) # radian
objective_function = (1.0 * abs(dist)) + (10.0 * abs(heading))
# rospy.logwarn('dist: %f, heading: %f, obj_func_result: %f', dist, heading, objective_function)
# dist tolerance: 0.170 meter, heading tolerance: +-0.09 rad (+-5.0 deg)
if objective_function >= 0.210:
self.cmd_vel.linear.x = (0.2 * dist) + (0.02 * (dist - self.priv_dist))
self.cmd_vel.linear.y = 0.0
self.cmd_vel.linear.z = 0.0
self.cmd_vel.angular.x = 0.0
self.cmd_vel.angular.y = 0.0
self.cmd_vel.angular.z = (1.0 * heading) + (0.01 * (heading - self.priv_heading))
self.cmd_vel_pub.publish(self.cmd_vel)
else:
self.cmd_vel.linear.x = 0.0
self.cmd_vel.angular.z = 0.0
self.cmd_vel_pub.publish(self.cmd_vel)
return 'succeeded'
self.priv_dist = dist
self.priv_heading = heading
def main():
rospy.init_node('pick_and_place_state_machine')
# namespace = rospy.get_param("~robot_name")
# planning_group = rospy.get_param("~planning_group")
namespace='om_with_tb3'
planning_group='arm'
# Create the sub SMACH state machine
task_center = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with task_center:
# Create the sub SMACH state machine
pick_center = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
with pick_center:
pick_center.userdata.planning_group = planning_group
def joint_position_request_cb(userdata, request):
joint = JointPosition()
joint.position = userdata.input_position
joint.max_velocity_scaling_factor = 1.0
joint.max_accelerations_scaling_factor = 1.0
request.planning_group = userdata.input_planning_group
request.joint_position = joint
return request
def joint_position_response_cb(userdata, response):
if response.is_planned == False:
return 'aborted'
else:
rospy.sleep(3.)
return 'succeeded'
def eef_pose_request_cb(userdata, request):
print(userdata.input_pose)
eef = KinematicsPose()
eef.pose = userdata.input_pose
rospy.loginfo('eef.position.x : %f', eef.pose.position.x)
rospy.loginfo('eef.position.y : %f', eef.pose.position.y)
rospy.loginfo('eef.position.z : %f', eef.pose.position.z)
eef.max_velocity_scaling_factor = 1.0
eef.max_accelerations_scaling_factor = 1.0
eef.tolerance = userdata.input_tolerance
request.planning_group = userdata.input_planning_group
request.kinematics_pose = eef
return request
def align_arm_with_object_response_cb(userdata, response):
if response.is_planned == False:
pick_center.userdata.align_arm_with_object_tolerance += 0.005
rospy.logwarn('Set more tolerance[%f]', pick_center.userdata.align_arm_with_object_tolerance)
return 'aborted'
else:
OFFSET_FOR_STRETCH = 0.030
pick_center.userdata.object_pose.position.x += OFFSET_FOR_STRETCH
rospy.sleep(3.)
return 'succeeded'
def close_to_object_response_cb(userdata, response):
if response.is_planned == False:
pick_center.userdata.close_to_object_tolerance += 0.005
rospy.logwarn('Set more tolerance[%f]', pick_center.userdata.close_to_object_tolerance)
return 'aborted'
else:
OFFSET_FOR_OBJECT_HEIGHT = 0.020
pick_center.userdata.object_pose.position.z += OFFSET_FOR_OBJECT_HEIGHT
rospy.sleep(3.)
return 'succeeded'
def pick_up_object_response_cb(userdata, response):
if response.is_planned == False:
pick_center.userdata.pick_up_object_tolerance += 0.005
rospy.logwarn('Set more tolerance[%f]', pick_center.userdata.pick_up_object_tolerance)
return 'aborted'
else:
rospy.sleep(3.)
return 'succeeded'
def gripper_request_cb(userdata, request):
joint = JointPosition()
joint.position = userdata.input_gripper
joint.max_velocity_scaling_factor = 1.0
joint.max_accelerations_scaling_factor = 1.0
request.planning_group = userdata.input_planning_group
request.joint_position = joint
return request
def gripper_response_cb(userdata, response):
rospy.sleep(1.)
return 'succeeded'
# init_pose
object_pose=Pose()
object_pose.position.x = 0.15
object_pose.position.y = 0.0
object_pose.position.z =0.20
object_pose.orientation.w = 1.0
object_pose.orientation.x = 0.0
object_pose.orientation.y = 0.0
object_pose.orientation.z = 0.0
pick_center.userdata.input_pose = object_pose
pick_center.userdata.align_arm_with_object_tolerance = 0.025
smach.StateMachine.add('INIT_POSITION',
ServiceState(planning_group + '/moveit/set_kinematics_pose',
SetKinematicsPose,
request_cb=eef_pose_request_cb,
response_cb=align_arm_with_object_response_cb,
input_keys=['input_planning_group',
'input_pose',
'input_tolerance']),
transitions={'succeeded':'OPEN_GRIPPER',
'aborted':'aborted'},
remapping={'input_planning_group':'planning_group',
'input_pose':'input_pose',
'input_tolerance':'align_arm_with_object_tolerance'})
## 2
pick_center.userdata.open_gripper = [0.008]
smach.StateMachine.add('OPEN_GRIPPER',
ServiceState(namespace + '/gripper',
SetJointPosition,
request_cb=gripper_request_cb,
response_cb=gripper_response_cb,
input_keys=['input_planning_group',
'input_gripper']),
transitions={'succeeded':'TOUCH_OUT'},
remapping={'input_planning_group':'planning_group',
'input_gripper':'open_gripper'})
# pick_center.userdata.init_position = [0.0, -0.65, 1.20, -0.54]
#[0.0,1.0,-0.3,-0.8]
pick_center.userdata.init_position =[0.0,0.8,-0.3,-0.6]
smach.StateMachine.add('TOUCH_OUT',
ServiceState(planning_group + '/moveit/set_joint_position',
SetJointPosition,
request_cb=joint_position_request_cb,
response_cb=joint_position_response_cb,
input_keys=['input_planning_group',
'input_position']),
transitions={'succeeded':'CLOSE_GRIPPER'},
remapping={'input_planning_group':'planning_group',
'input_position':'init_position'})
pick_center.userdata.close_gripper = [-0.000]
smach.StateMachine.add('CLOSE_GRIPPER',
ServiceState(namespace + '/gripper',
SetJointPosition,
request_cb=gripper_request_cb,
response_cb=gripper_response_cb,
input_keys=['input_planning_group',
'input_gripper']),
transitions={'succeeded':'TOUCH_BACK','aborted':'TOUCH_BACK'},
remapping={'input_planning_group':'planning_group',
'input_gripper':'close_gripper'})
pick_center.userdata.object_pose = Pose()
# smach.StateMachine.add('GET_POSE_OF_THE_OBJECT', getPoseOfTheObject(),
# transitions={'succeeded':'ALIGN_ARM_WITH_OBJECT',
# 'aborted':'aborted'},
# remapping={'output_object_pose':'object_pose'})
# object_pose.position = self.ar_marker_pose.pose.pose.position
object_pose=Pose()
object_pose.position.x = 0.15
object_pose.position.y = 0.0
object_pose.position.z =0.35
object_pose.orientation.w = 1.0
object_pose.orientation.x = 0.0
object_pose.orientation.y = 0.0
object_pose.orientation.z = 0.0
pick_center.userdata.input_pose = object_pose
pick_center.userdata.align_arm_with_object_tolerance = 0.025
smach.StateMachine.add('TOUCH_BACK',
ServiceState(planning_group + '/moveit/set_kinematics_pose',
SetKinematicsPose,
request_cb=eef_pose_request_cb,
response_cb=align_arm_with_object_response_cb,
input_keys=['input_planning_group',
'input_pose',
'input_tolerance']),
transitions={'succeeded':'TURN',
'aborted':'aborted'},
remapping={'input_planning_group':'planning_group',
'input_pose':'input_pose',
'input_tolerance':'align_arm_with_object_tolerance'})
pick_center.userdata.target__pose =[1.57,0.8,-0.3,-0.6]
smach.StateMachine.add('TURN',
ServiceState(planning_group + '/moveit/set_joint_position',
SetJointPosition,
request_cb=joint_position_request_cb,
response_cb=joint_position_response_cb,
input_keys=['input_planning_group',
'input_position']),
transitions={'succeeded':'OPEN_GRIPPER_2'},
remapping={'input_planning_group':'planning_group',
'input_position':'target__pose'})
## 2
pick_center.userdata.open_gripper = [0.008]
smach.StateMachine.add('OPEN_GRIPPER_2',
ServiceState(namespace + '/gripper',
SetJointPosition,
request_cb=gripper_request_cb,
response_cb=gripper_response_cb,
input_keys=['input_planning_group',
'input_gripper']),
transitions={'succeeded':'succeeded'},
remapping={'input_planning_group':'planning_group',
'input_gripper':'open_gripper'})
#-------------------------------------
smach.StateMachine.add('ALIGN_ARM_WITH_OBJECT',
ServiceState(planning_group + '/moveit/set_kinematics_pose',
SetKinematicsPose,
request_cb=eef_pose_request_cb,
response_cb=align_arm_with_object_response_cb,
input_keys=['input_planning_group',
'input_pose',
'input_tolerance']),
transitions={'succeeded':'OPEN_GRIPPER',
'aborted':'aborted'},
remapping={'input_planning_group':'planning_group',
'input_pose':'object_pose',
'input_tolerance':'align_arm_with_object_tolerance'})
pick_center.userdata.close_to_object_tolerance = 0.01
smach.StateMachine.add('CLOSE_TO_OBJECT',
ServiceState(planning_group + '/moveit/set_kinematics_pose',
SetKinematicsPose,
request_cb=eef_pose_request_cb,
response_cb=close_to_object_response_cb,
input_keys=['input_planning_group',
'input_pose',
'input_tolerance']),
transitions={'succeeded':'succeeded',
'aborted':'CLOSE_TO_OBJECT'},
remapping={'input_planning_group':'planning_group',
'input_pose':'object_pose',
'input_tolerance':'close_to_object_tolerance'})
# pick_center.userdata.close_gripper = [0.0]
# smach.StateMachine.add('GRIP_OBJECT',
# ServiceState(namespace + '/gripper',
# SetJointPosition,
# request_cb=gripper_request_cb,
# response_cb=gripper_response_cb,
# input_keys=['input_planning_group',
# 'input_gripper']),
# transitions={'succeeded':'PICK_UP_OBJECT'},
# remapping={'input_planning_group':'planning_group',
# 'input_gripper':'close_gripper'})
# pick_center.userdata.pick_up_object_tolerance = 0.01
# smach.StateMachine.add('PICK_UP_OBJECT',
# ServiceState(planning_group + '/moveit/set_kinematics_pose',
# SetKinematicsPose,
# request_cb=eef_pose_request_cb,
# response_cb=pick_up_object_response_cb,
# input_keys=['input_planning_group',
# 'input_pose',
# 'input_tolerance']),
# transitions={'succeeded':'SET_HOLDING_POSITION',
# 'aborted':'PICK_UP_OBJECT'},
# remapping={'input_planning_group':'planning_group',
# 'input_pose':'object_pose',
# 'input_tolerance':'pick_up_object_tolerance'})
# pick_center.userdata.holding_position = [0.0, -1.5707, 1.37, -0.20]
# smach.StateMachine.add('SET_HOLDING_POSITION',
# ServiceState(planning_group + '/moveit/set_joint_position',
# SetJointPosition,
# request_cb=joint_position_request_cb,
# response_cb=joint_position_response_cb,
# input_keys=['input_planning_group',
# 'input_position']),
# transitions={'succeeded':'succeeded'},
# remapping={'input_planning_group':'planning_group',
# 'input_position':'holding_position'})
smach.StateMachine.add('PICK', pick_center,
transitions={'succeeded':'succeeded', 'aborted':'aborted'})
sis = smach_ros.IntrospectionServer('server_name', task_center, '/TASKS_CENTER')
sis.start()
# Execute SMACH plan
outcome = task_center.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
if __name__ == '__main__':
main()
Ar码识别抓取的demo