Smatch 最基础的应用
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