def handle_result(msg):
result = ExecuteTrajectoryResult.from_msg(msg)
if result.error_code != MoveItErrorCodes.SUCCESS:
ros_error = RosError('Execute trajectory failed. Message={}'.format(result.error_code.human_readable), int(result.error_code))
action_result._set_error_result(ros_error)
if errback:
errback(ros_error)
else:
action_result._set_result(result)
if callback:
callback(result)
def handle_result(msg):
result = FollowJointTrajectoryResult.from_msg(msg)
if result.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
ros_error = RosError(
'Follow trajectory failed={}'.format(result.error_string),
int(result.error_code))
action_result._set_error_result(ros_error)
if errback:
errback(ros_error)
else:
action_result._set_result(result)
if callback:
callback(result)
def validate_response(response):
"""Raise an exception if the response indicates an error condition."""
if response.error_code != MoveItErrorCodes.SUCCESS:
raise RosError(response.error_code.human_readable,
int(response.error_code))
def execute_joint_trajectory(self,
joint_trajectory,
action_name='/execute_trajectory',
callback=None,
errback=None,
feedback_callback=None,
timeout=60000):
"""Execute a joint trajectory via the MoveIt infrastructure.
Note
----
This method does not support Multi-DOF Joint Trajectories.
Parameters
----------
joint_trajectory : :class:`compas_fab.robots.JointTrajectory`
Instance of joint trajectory.
callback : callable
Function to be invoked when the goal is completed, requires
one positional parameter ``result``.
action_name : string
ROS action name, defaults to ``/execute_trajectory``.
errback : callable
Function to be invoked in case of error or timeout, requires
one position parameter ``exception``.
feedback_callback : callable
Function to be invoked during execution to provide feedback.
timeout : int
Timeout for goal completion in milliseconds.
Returns
-------
:class:`CancellableFutureResult`
An instance of a cancellable future result.
"""
joint_trajectory = self._convert_to_ros_trajectory(joint_trajectory)
trajectory = RobotTrajectory(joint_trajectory=joint_trajectory)
trajectory_goal = ExecuteTrajectoryGoal(trajectory=trajectory)
action_client = ActionClient(self, action_name,
'moveit_msgs/ExecuteTrajectoryAction')
goal = Goal(action_client, Message(trajectory_goal.msg))
action_result = CancellableRosActionResult(goal)
def handle_result(msg):
result = ExecuteTrajectoryResult.from_msg(msg)
if result.error_code != MoveItErrorCodes.SUCCESS:
ros_error = RosError(
'Execute trajectory failed. Message={}'.format(
result.error_code.human_readable),
int(result.error_code))
action_result._set_error_result(ros_error)
if errback:
errback(ros_error)
else:
action_result._set_result(result)
if callback:
callback(result)
def handle_feedback(msg):
feedback = ExecuteTrajectoryFeedback.from_msg(msg)
feedback_callback(feedback)
def handle_failure(error):
errback(error)
goal.on('result', handle_result)
if feedback_callback:
goal.on('feedback', handle_feedback)
if errback:
goal.on(
'timeout',
lambda: handle_failure(RosError('Action Goal timeout', -1)))
goal.send(timeout=timeout)
return action_result
def follow_joint_trajectory(self,
joint_trajectory,
action_name='/joint_trajectory_action',
callback=None,
errback=None,
feedback_callback=None,
timeout=60000):
"""Follow the joint trajectory as computed by MoveIt planner.
Parameters
----------
joint_trajectory : :class:`compas_fab.robots.JointTrajectory`
Instance of joint trajectory. Note: for backwards compatibility, this supports a ROS Msg being passed as well.
action_name : string
ROS action name, defaults to ``/joint_trajectory_action`` but some drivers need ``/follow_joint_trajectory``.
callback : callable
Function to be invoked when the goal is completed, requires
one positional parameter ``result``.
errback : callable
Function to be invoked in case of error or timeout, requires
one position parameter ``exception``.
feedback_callback : callable
Function to be invoked during execution to provide feedback.
timeout : int
Timeout for goal completion in milliseconds.
Returns
-------
:class:`CancellableTask`
An instance of a cancellable tasks.
"""
joint_trajectory = self._convert_to_ros_trajectory(joint_trajectory)
trajectory_goal = FollowJointTrajectoryGoal(
trajectory=joint_trajectory)
action_client = ActionClient(
self, action_name, 'control_msgs/FollowJointTrajectoryAction')
goal = Goal(action_client, Message(trajectory_goal.msg))
action_result = CancellableRosActionResult(goal)
def handle_result(msg):
result = FollowJointTrajectoryResult.from_msg(msg)
if result.error_code != FollowJointTrajectoryResult.SUCCESSFUL:
ros_error = RosError(
'Follow trajectory failed={}'.format(result.error_string),
int(result.error_code))
action_result._set_error_result(ros_error)
if errback:
errback(ros_error)
else:
action_result._set_result(result)
if callback:
callback(result)
def handle_feedback(msg):
feedback = FollowJointTrajectoryFeedback.from_msg(msg)
feedback_callback(feedback)
def handle_failure(error):
errback(error)
goal.on('result', handle_result)
if feedback_callback:
goal.on('feedback', handle_feedback)
if errback:
goal.on(
'timeout',
lambda: handle_failure(RosError('Action Goal timeout', -1)))
goal.send(timeout=timeout)
return action_result
def follow_joint_trajectory(self,
joint_trajectory,
callback=None,
errback=None,
feedback_callback=None,
timeout=60000):
"""Follow the joint trajectory as computed by MoveIt planner.
Parameters
----------
joint_trajectory : JointTrajectory
Joint trajectory message as computed by MoveIt planner
callback : callable
Function to be invoked when the goal is completed, requires
one positional parameter ``result``.
errback : callable
Function to be invoked in case of error or timeout, requires
one position parameter ``exception``.
feedback_callback : callable
Function to be invoked during execution to provide feedback.
timeout : int
Timeout for goal completion in milliseconds.
Returns
-------
:class:`CancellableTask`
An instance of a cancellable tasks.
"""
trajectory_goal = FollowJointTrajectoryGoal(
trajectory=joint_trajectory)
def handle_result(msg, client):
result = FollowJointTrajectoryResult.from_msg(msg)
callback(result)
def handle_failure(error):
errback(error)
connection_timeout = 3000
action_client = ActionClient(
self,
'/joint_trajectory_action', # '/follow_joint_trajectory',
'control_msgs/FollowJointTrajectoryAction',
connection_timeout)
goal = Goal(action_client, Message(trajectory_goal.msg))
if callback:
goal.on('result',
lambda result: handle_result(result, action_client))
if feedback_callback:
goal.on('feedback', feedback_callback)
if errback:
goal.on(
'timeout',
lambda: handle_failure(RosError("Action Goal timeout", -1)))
action_client.on(
'timeout', lambda: handle_failure(
RosError("Actionlib client timeout", -1)))
goal.send(timeout=timeout)
return CancellableRosAction(goal)
