{% extends "ROSBase.tpl.py" %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ super() }}
{{ from_import(defined_headers, 'smacha_ros.srv', 'GripperSrv') }}
{{ from_import(defined_headers, 'smacha_ros.srv', 'GripperSrvRequest') }}
{{ from_import(defined_headers, 'smacha_ros.srv', 'GripperSrvResponse') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', '*') }}
{% endblock imports %}
{% block defs %}
{{ super() }}
def gripper_srv(req):
if req.max_effort > 5.0:
print('gripper_srv() returning True')
return GripperSrvResponse(True)
else:
print('gripper_srv() returning False')
return GripperSrvResponse(False)
{% endblock defs %}
{% block main_def %}
{{ super() }}
# Register a gripper service
s = rospy.Service('gripper_srv', GripperSrv, gripper_srv)
{% endblock main_def %}
{% block base_footer %}
{{ super() }}
type, or as a [[3],[4]] list, and returns a geometry_msgs.msg.Pose type.
language: Python
framework: SMACH
type: None
tags: [core]
includes: []
extends: []
variables: []
input_keys: []
output_keys: []
{% endblock meta %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Pose') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'PoseStamped') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Point') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Quaternion') }}
{% endblock imports %}
{% block defs %}
{% if 'def_parse_pose' not in defined_headers %}
def parse_pose(pose_input):
"""
Parse pose_input into Pose.
"""
try:
assert isinstance(pose_input, Pose)
return pose_input
except:
geometry_msgs.msg.TransformStamped type.
language: Python
framework: SMACH
type: None
tags: [core]
includes: []
extends: []
variables: []
input_keys: []
output_keys: []
{% endblock meta %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Transform') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'TransformStamped') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Vector3') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Quaternion') }}
{% endblock imports %}
{% block defs %}
{% if 'def_parse_transformstamped' not in defined_headers %}
def parse_transformstamped(transform_input):
"""
Parse transform_input into TransformStamped.
"""
try:
assert isinstance(transform_input, TransformStamped)
return transform_input
except:
{% extends "SimpleActionState.tpl.py" %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ from_import(defined_headers, 'smacha_ros.msg', 'TestAction') }}
{{ from_import(defined_headers, 'smacha_ros.msg', 'TestGoal') }}
{{ super() }}
{% endblock imports %}
{% block header %}
{% endblock header %}
{% block body %}
# Add another simple action state. This will give a goal
# that should abort the action state when it is received, so we
# map 'aborted' for this state onto 'succeeded' for the state machine.
{{ super() }}
{% endblock body %}
type or as a [3] list, and returns a geometry_msgs.msg.Point type.
language: Python
framework: SMACH
type: None
tags: [core]
includes: []
extends: []
variables: []
input_keys: []
output_keys: []
{% endblock meta %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Point') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Point32') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'PointStamped') }}
{% endblock imports %}
{% block defs %}
{% if 'def_parse_pointstamped' not in defined_headers %}
def parse_pointstamped(point_input):
"""
Parse point_input into PointStamped.
"""
try:
assert isinstance(point_input, PointStamped)
return point_input
except:
pass
{% extends "ROSBase.tpl.py" %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ super() }}
{{ from_import(defined_headers, 'smacha_ros.msg', 'TestAction') }}
{{ from_import(defined_headers, 'actionlib_msgs.msg', '*') }}
{% endblock imports %}
{% block class_defs %}
{{ super() }}
# Create a trivial action server
class TestServer:
def __init__(self,name):
self._sas = SimpleActionServer(name,
TestAction,
execute_cb=self.execute_cb)
def execute_cb(self, msg):
if msg.goal == 0:
self._sas.set_succeeded()
elif msg.goal == 1:
self._sas.set_aborted()
elif msg.goal == 2:
self._sas.set_preempted()
{% endblock class_defs %}
{% block main_def %}
{{ super() }}
# Start an action server
{% extends "ServiceState.tpl.py" %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ from_import(defined_headers, 'smacha_ros.srv', 'GripperSrvRequest') }}
{{ super() }}
{% endblock imports %}
{% block body %}
@smach.cb_interface(input_keys=['gripper_input'])
def gripper_request_cb(userdata, request):
gripper_request = GripperSrvRequest()
gripper_request.position.x = 2.0
gripper_request.max_effort = userdata.gripper_input
return gripper_request
{{ super() }}
{% endblock body %}
The definitions for the userdata keys named in the input_keys and
output_keys variables.
type: dict of str
outcomes:
- succeeded
- aborted
- preempted
{% endblock meta %}
{% from "Utils.tpl.py" import from_import, render_goal_slots, render_input_keys, render_result_slots, render_output_keys, render_transitions, render_remapping %}
{% extends "State.tpl.py" %}
{% block imports %}
{{ super() }}
{{ from_import(defined_headers, 'actionlib', '*') }}
{% endblock imports %}
{% block body %}
smach.{{ parent_type }}.add('{{ name }}',
{{ '' | indent(23, true) }}smach_ros.SimpleActionState('{{ action_server_namespace }}', {{ action }}{% if goal is defined %},
{{ 'goal = ' | indent(51, true) }}{{ goal }}{% elif goal_slots is defined %},
{{ render_goal_slots(goal_slots) }}{% elif goal_cb is defined %},
{{ 'goal_cb = ' | indent(51, true) }}{{ goal_cb }}{% if input_keys is defined %},
{{ render_input_keys(input_keys) }}{% endif %}{% endif %}{% if result_slots is defined %},
{{ render_result_slots(result_slots) }}{% elif result_cb is defined %},
{{ 'result_cb = ' | indent(51, true) }}{{ result_cb }}{% if output_keys is defined %},
{{ render_output_keys(output_keys) }}{% endif %}{% endif %}){% if transitions is defined %},
{{ render_transitions(transitions) }}{% endif %}{% if remapping is defined %},
{{ render_remapping(remapping) }}{% endif %})
{% endblock body %}
{% extends "State.tpl.py" %}
{% include "ParseTransformStamped.tpl.py" %}
{% include "ParsePointStamped.tpl.py" %}
{% include "ParsePose.tpl.py" %}
{% include "ParsePoseStamped.tpl.py" %}
{% include "ParsePoseArray.tpl.py" %}
{% include "ParsePointCloud.tpl.py" %}
{% include "ParsePointCloud2.tpl.py" %}
{% include "TF2ListenerSingleton.tpl.py" %}
{% block imports %}
{{ super() }}
{{ import_module_as(defined_headers, 'numpy', 'np') }}
{{ from_import(defined_headers, 'tf2_geometry_msgs', 'do_transform_point') }}
{{ from_import(defined_headers, 'tf2_geometry_msgs', 'do_transform_pose') }}
{{ from_import(defined_headers, 'tf2_sensor_msgs', 'do_transform_cloud') }}
{% endblock imports %}
{% block class_defs %}
{{ super() }}
{% if 'class_TransformMsgState' not in defined_headers %}
class TransformMsgState(smach.State):
def __init__(self, transform_type='transform',
input_keys=['msg', 'frame_id'], output_keys=['output_msg'], callbacks=None):
smach.State.__init__(self, input_keys=input_keys, output_keys=output_keys, outcomes=['succeeded', 'aborted'])
# Save transform type
self._transform_type = transform_type
and returns a sensor_msgs.msg.PointCloud type.
language: Python
framework: SMACH
type: None
tags: [core]
includes: []
extends: []
variables: []
input_keys: []
output_keys: []
{% endblock meta %}
{% from "Utils.tpl.py" import from_import, from_import_as %}
{% block imports %}
{{ from_import(defined_headers, 'sensor_msgs.msg', 'PointCloud') }}
{{ from_import(defined_headers, 'geometry_msgs.msg', 'Point32') }}
{{ from_import_as(defined_headers, 'sensor_msgs', 'point_cloud2', 'pc2') }}
{% endblock imports %}
{% block defs %}
{% if 'def_parse_pointcloud' not in defined_headers %}
def parse_pointcloud(pointcloud_input):
"""
Parse pointcloud_input into PointCloud.
"""
try:
assert isinstance(pointcloud_input, PointCloud)
return pointcloud_input
except:
pass
and returns a sensor_msgs.msg.PointCloud2 type.
language: Python
framework: SMACH
type: None
tags: [core]
includes: []
extends: []
variables: []
input_keys: []
output_keys: []
{% endblock meta %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ from_import(defined_headers, 'sensor_msgs.msg', 'PointCloud') }}
{{ from_import(defined_headers, 'sensor_msgs.msg', 'PointCloud2') }}
{{ from_import(defined_headers, 'sensor_msgs.point_cloud2', 'create_cloud_xyz32') }}
{% endblock imports %}
{% block defs %}
{% if 'def_parse_pointcloud2' not in defined_headers %}
def parse_pointcloud2(pointcloud_input):
"""
Parse pointcloud_input into PointCloud2.
"""
try:
assert isinstance(pointcloud_input, PointCloud2)
return pointcloud_input
except:
pass
trajectory_msgs.msg.JointTrajectoryPoint type.
language: Python
framework: SMACH
type: None
tags: [core]
includes: []
extends: []
variables: []
input_keys: []
output_keys: []
{% endblock meta %}
{% from "Utils.tpl.py" import from_import %}
{% block imports %}
{{ from_import(defined_headers, 'sensor_msgs.msg', 'JointState') }}
{{ from_import(defined_headers, 'trajectory_msgs.msg', 'JointTrajectoryPoint') }}
{% endblock imports %}
{% block defs %}
{% if 'def_parse_jointtrajectorypoint' not in defined_headers %}
def parse_jointtrajectorypoint(jointtrajectorypoint_input):
"""
Parse jointtrajectorypoint_input into a JointTrajectoryPoint.
"""
try:
assert isinstance(jointtrajectorypoint_input, JointTrajectoryPoint)
return jointtrajectorypoint_input
except:
pass
try:
