class MoveItPlanner(PlannerBackend):
"""Implement the planner backend interface based on MoveIt!
"""
GET_POSITION_IK = ServiceDescription('/compute_ik', 'GetPositionIK',
GetPositionIKRequest,
GetPositionIKResponse,
validate_response)
GET_POSITION_FK = ServiceDescription('/compute_fk', 'GetPositionFK',
GetPositionFKRequest,
GetPositionFKResponse,
validate_response)
GET_CARTESIAN_PATH = ServiceDescription('/compute_cartesian_path',
'GetCartesianPath',
GetCartesianPathRequest,
GetCartesianPathResponse,
validate_response)
GET_MOTION_PLAN = ServiceDescription('/plan_kinematic_path',
'GetMotionPlan', MotionPlanRequest,
MotionPlanResponse, validate_response)
GET_PLANNING_SCENE = ServiceDescription('/get_planning_scene',
'GetPlanningScene',
GetPlanningSceneRequest,
GetPlanningSceneResponse)
def init_planner(self):
self.collision_object_topic = Topic(self,
'/collision_object',
'moveit_msgs/CollisionObject',
queue_size=None)
self.collision_object_topic.advertise()
self.attached_collision_object_topic = Topic(
self,
'/attached_collision_object',
'moveit_msgs/AttachedCollisionObject',
queue_size=None)
self.attached_collision_object_topic.advertise()
def dispose_planner(self):
if hasattr(self,
'collision_object_topic') and self.collision_object_topic:
self.collision_object_topic.unadvertise()
if hasattr(self, 'attached_collision_object_topic'
) and self.attached_collision_object_topic:
self.attached_collision_object_topic.unadvertise()
def _convert_constraints_to_rosmsg(self, constraints, header):
"""Convert COMPAS FAB constraints into ROS Messages."""
if not constraints:
return None
ros_constraints = Constraints()
for c in constraints:
if c.type == c.JOINT:
ros_constraints.joint_constraints.append(
JointConstraint.from_joint_constraint(c))
elif c.type == c.POSITION:
ros_constraints.position_constraints.append(
PositionConstraint.from_position_constraint(header, c))
elif c.type == c.ORIENTATION:
ros_constraints.orientation_constraints.append(
OrientationConstraint.from_orientation_constraint(
header, c))
else:
raise NotImplementedError
return ros_constraints
# ==========================================================================
# planning services
# ==========================================================================
def inverse_kinematics_async(self,
callback,
errback,
robot,
frame,
group,
start_configuration,
avoid_collisions=True,
constraints=None,
attempts=8,
attached_collision_meshes=None):
"""Asynchronous handler of MoveIt IK service."""
base_link = robot.model.root.name
header = Header(frame_id=base_link)
pose = Pose.from_frame(frame)
pose_stamped = PoseStamped(header, pose)
joint_state = JointState(name=start_configuration.joint_names,
position=start_configuration.values,
header=header)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_meshes:
for acm in attached_collision_meshes:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
constraints = self._convert_constraints_to_rosmsg(constraints, header)
ik_request = PositionIKRequest(group_name=group,
robot_state=start_state,
constraints=constraints,
pose_stamped=pose_stamped,
avoid_collisions=avoid_collisions,
attempts=attempts)
def convert_to_positions(response):
callback((response.solution.joint_state.position,
response.solution.joint_state.name))
self.GET_POSITION_IK(self, (ik_request, ), convert_to_positions,
errback)
def forward_kinematics_async(self, callback, errback, robot, configuration,
group, ee_link):
"""Asynchronous handler of MoveIt FK service."""
base_link = robot.model.root.name
header = Header(frame_id=base_link)
fk_link_names = [ee_link]
joint_state = JointState(name=configuration.joint_names,
position=configuration.values,
header=header)
robot_state = RobotState(joint_state,
MultiDOFJointState(header=header))
def convert_to_frame(response):
callback(response.pose_stamped[0].pose.frame)
self.GET_POSITION_FK(self, (header, fk_link_names, robot_state),
convert_to_frame, errback)
def plan_cartesian_motion_async(self, callback, errback, robot, frames,
start_configuration, group, max_step,
jump_threshold, avoid_collisions,
path_constraints,
attached_collision_meshes):
"""Asynchronous handler of MoveIt cartesian motion planner service."""
base_link = robot.model.root.name # use world coords
ee_link = robot.get_end_effector_link_name(group)
header = Header(frame_id=base_link)
waypoints = [Pose.from_frame(frame) for frame in frames]
joint_state = JointState(header=header,
name=start_configuration.joint_names,
position=start_configuration.values)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_meshes:
for acm in attached_collision_meshes:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
path_constraints = self._convert_constraints_to_rosmsg(
path_constraints, header)
request = dict(header=header,
start_state=start_state,
group_name=group,
link_name=ee_link,
waypoints=waypoints,
max_step=float(max_step),
jump_threshold=float(jump_threshold),
avoid_collisions=bool(avoid_collisions),
path_constraints=path_constraints)
def convert_to_trajectory(response):
try:
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = response.fraction
trajectory.joint_names = response.solution.joint_trajectory.joint_names
joint_types = robot.get_joint_types_by_names(
trajectory.joint_names)
trajectory.points = convert_trajectory_points(
response.solution.joint_trajectory.points, joint_types)
start_state = response.start_state.joint_state
start_state_types = robot.get_joint_types_by_names(
start_state.name)
trajectory.start_configuration = Configuration(
start_state.position, start_state_types, start_state.name)
callback(trajectory)
except Exception as e:
errback(e)
self.GET_CARTESIAN_PATH(self, request, convert_to_trajectory, errback)
def plan_motion_async(self,
callback,
errback,
robot,
goal_constraints,
start_configuration,
group,
path_constraints=None,
trajectory_constraints=None,
planner_id='',
num_planning_attempts=8,
allowed_planning_time=2.,
max_velocity_scaling_factor=1.,
max_acceleration_scaling_factor=1.,
attached_collision_meshes=None,
workspace_parameters=None):
"""Asynchronous handler of MoveIt motion planner service."""
# http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html
# TODO: if list of frames (goals) => receive multiple solutions?
base_link = robot.model.root.name # use world coords
header = Header(frame_id=base_link)
joint_state = JointState(header=header,
name=start_configuration.joint_names,
position=start_configuration.values)
start_state = RobotState(joint_state,
MultiDOFJointState(header=header))
if attached_collision_meshes:
for acm in attached_collision_meshes:
aco = AttachedCollisionObject.from_attached_collision_mesh(acm)
start_state.attached_collision_objects.append(aco)
# convert constraints
goal_constraints = [
self._convert_constraints_to_rosmsg(goal_constraints, header)
]
path_constraints = self._convert_constraints_to_rosmsg(
path_constraints, header)
if trajectory_constraints is not None:
trajectory_constraints = TrajectoryConstraints(
constraints=self._convert_constraints_to_rosmsg(
path_constraints, header))
request = dict(
start_state=start_state,
goal_constraints=goal_constraints,
path_constraints=path_constraints,
trajectory_constraints=trajectory_constraints,
planner_id=planner_id,
group_name=group,
num_planning_attempts=num_planning_attempts,
allowed_planning_time=allowed_planning_time,
max_velocity_scaling_factor=max_velocity_scaling_factor,
max_acceleration_scaling_factor=max_velocity_scaling_factor)
# workspace_parameters=workspace_parameters
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
trajectory.planning_time = response.planning_time
joint_types = robot.get_joint_types_by_names(
trajectory.joint_names)
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
start_state = response.trajectory_start.joint_state
start_state_types = robot.get_joint_types_by_names(
start_state.name)
trajectory.start_configuration = Configuration(
start_state.position, start_state_types, start_state.name)
callback(trajectory)
self.GET_MOTION_PLAN(self, request, convert_to_trajectory, errback)
# ==========================================================================
# collision objects
# ==========================================================================
def get_planning_scene_async(self, callback, errback):
request = dict(components=PlanningSceneComponents(
PlanningSceneComponents.SCENE_SETTINGS
| PlanningSceneComponents.ROBOT_STATE
| PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS
| PlanningSceneComponents.WORLD_OBJECT_NAMES
| PlanningSceneComponents.WORLD_OBJECT_GEOMETRY
| PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.OBJECT_COLORS))
self.GET_PLANNING_SCENE(self, request, callback, errback)
def add_collision_mesh(self, collision_mesh):
"""Add a collision mesh to the planning scene."""
co = CollisionObject.from_collision_mesh(collision_mesh)
self._collision_object(co, CollisionObject.ADD)
def remove_collision_mesh(self, id):
"""Remove a collision mesh from the planning scene."""
co = CollisionObject()
co.id = id
self._collision_object(co, CollisionObject.REMOVE)
def append_collision_mesh(self, collision_mesh):
"""Append a collision mesh to the planning scene."""
co = CollisionObject.from_collision_mesh(collision_mesh)
self._collision_object(co, CollisionObject.APPEND)
def _collision_object(self,
collision_object,
operation=CollisionObject.ADD):
if not hasattr(
self,
'collision_object_topic') or not self.collision_object_topic:
self.init_planner()
collision_object.operation = operation
self.collision_object_topic.publish(collision_object.msg)
def add_attached_collision_mesh(self, attached_collision_mesh):
"""Add a collision mesh attached to the robot."""
aco = AttachedCollisionObject.from_attached_collision_mesh(
attached_collision_mesh)
self._attached_collision_object(aco, operation=CollisionObject.ADD)
def remove_attached_collision_mesh(self, id):
"""Add an attached collision mesh from the robot."""
aco = AttachedCollisionObject()
aco.object.id = id
return self._attached_collision_object(
aco, operation=CollisionObject.REMOVE)
def _attached_collision_object(self,
attached_collision_object,
operation=CollisionObject.ADD):
if not hasattr(self, 'attached_collision_object_topic'
) or not self.attached_collision_object_topic:
self.init_planner()
attached_collision_object.object.operation = operation
self.attached_collision_object_topic.publish(
attached_collision_object.msg)
import time
from roslibpy import Message
from roslibpy import Topic
from compas_fab.backends import RosClient
with RosClient('localhost') as client:
talker = Topic(client, '/chatter', 'std_msgs/String')
talker.advertise()
while client.is_connected:
talker.publish(Message({'data': 'Hello World!'}))
print('Sending message...')
time.sleep(1)
talker.unadvertise()
class DirectUrActionClient(EventEmitterMixin):
"""Direct UR Script action client to simulate an action interface
on arbitrary URScript commands.
"""
def __init__(self, ros, timeout=None,
omit_feedback=False, omit_status=False, omit_result=False):
super(DirectUrActionClient, self).__init__()
self.server_name = '/ur_driver/URScript'
self.ros = ros
self.timeout = timeout
self.omit_feedback = omit_feedback
self.omit_status = omit_status
self.omit_result = omit_result
self.goal = None
self._received_status = False
# Advertise the UR Script topic
self._urscript_topic = Topic(ros, self.server_name, 'std_msgs/String')
self._urscript_topic.advertise()
# Create the topics associated with actionlib
self.status_listener = Topic(ros, '/tool_velocity', 'geometry_msgs/TwistStamped')
# Subscribe to the status topic
if not self.omit_status:
self.status_listener.subscribe(self._on_status_message)
# If timeout specified, emit a 'timeout' event if the action server does not respond
if self.timeout:
self.ros.call_later(self.timeout / 1000., self._trigger_timeout)
self._internal_state = 'idle'
def _on_status_message(self, message):
self._received_status = True
self.goal.emit('status', message)
twist = message['twist']
total_velocity = math.fsum(list(twist['linear'].values()) + list(twist['angular'].values()))
in_motion = total_velocity != 0.0
if self._internal_state == 'idle':
if in_motion:
self._internal_state = 'executing_goal'
elif self._internal_state == 'executing_goal':
if not in_motion:
self._internal_state = 'stopped'
self.goal.emit('result', message)
def _trigger_timeout(self):
if not self._received_status:
self.emit('timeout')
def add_goal(self, goal):
"""Add a goal to this action client.
Args:
goal (:class:`.Goal`): Goal to add.
"""
self.goal = goal
def cancel(self):
"""Cancel all goals associated with this action client."""
pass
# self.cancel_topic.publish(Message())
def dispose(self):
"""Unsubscribe and unadvertise all topics associated with this action client."""
# And the UR Script topic
self._urscript_topic.unadvertise()
if not self.omit_status:
self.status_listener.unsubscribe(self._on_status_message)
def send_goal(self, goal, result_callback=None, timeout=None):
"""Send goal to the action server.
Args:
goal (:class:`URGoal`): The goal containing the script lines
timeout (:obj:`int`): Timeout for the goal's result expressed in milliseconds.
callback (:obj:`callable`): Function to be called when a result is received. It is a shorthand for hooking on the ``result`` event.
"""
self._internal_state == 'idle'
if result_callback:
goal.on('result', result_callback)
ur_script = goal.goal_message['goal']['script']
message = Message({'data': ur_script})
self._urscript_topic.publish(message)
if timeout:
self.ros.call_later(
timeout / 1000., goal._trigger_timeout)
