cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Fixed whitespacing with multiline comments

pull/5/head
hello-binit 4 years ago
parent
876df63371
commit
240ee88b92
2 changed files with 90 additions and 64 deletions
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +71
    -43
      stretch_core/nodes/command_groups.py
  2. +19
    -21
      stretch_core/nodes/joint_trajectory_server.py

+ 71
- 43
stretch_core/nodes/command_groups.py View File

@ -99,7 +99,9 @@ class SimpleCommandGroup:
self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
if self.goal['position'] is None: if self.goal['position'] is None:
err_str = "Received goal point with positions array length={0}. This joint ({1})'s index is {2}. Length of array must cover all joints listed in commanded_joint_names.".format(len(point.positions), self.name, self.index)
err_str = ("Received goal point with positions array length={0}. "
"This joint ({1})'s index is {2}. Length of array must cover all joints listed "
"in commanded_joint_names.").format(len(point.positions), self.name, self.index)
invalid_goal_callback(err_str) invalid_goal_callback(err_str)
return False return False
@ -225,7 +227,10 @@ class WristYawCommandGroup(SimpleCommandGroup):
def init_execution(self, robot, robot_status, **kwargs): def init_execution(self, robot, robot_status, **kwargs):
if self.active: if self.active:
robot.end_of_arm.move_by('wrist_yaw', self.update_execution(robot_status)[1], v_r=self.goal['velocity'], a_r=self.goal['acceleration'])
robot.end_of_arm.move_by('wrist_yaw',
self.update_execution(robot_status)[1],
v_r=self.goal['velocity'],
a_r=self.goal['acceleration'])
def update_execution(self, robot_status, **kwargs): def update_execution(self, robot_status, **kwargs):
self.error = None self.error = None
@ -248,11 +253,11 @@ class GripperCommandGroup(SimpleCommandGroup):
self.index = None self.index = None
active_gripper_joint_names = list(set(commanded_joint_names) & set(self.gripper_joint_names)) active_gripper_joint_names = list(set(commanded_joint_names) & set(self.gripper_joint_names))
if len(active_gripper_joint_names) > 1: if len(active_gripper_joint_names) > 1:
err_str = 'Received a command for the gripper that includes more than one gripper joint name: {0}. \
Only one joint name is allowed to be used for a gripper command to avoid conflicts \
and confusion. The gripper only has a single degree of freedom that can be \
controlled using the following three mutually exclusive joint names: \
{1}.'.format(active_gripper_joint_names, self.gripper_joint_names)
err_str = ("Received a command for the gripper that includes more than one gripper joint name: {0}. "
"Only one joint name is allowed to be used for a gripper command to avoid conflicts "
"and confusion. The gripper only has a single degree of freedom that can be "
"controlled using the following three mutually exclusive joint names: "
"{1}.").format(active_gripper_joint_names, self.gripper_joint_names)
invalid_joints_callback(err_str) invalid_joints_callback(err_str)
return False return False
elif len(active_gripper_joint_names) == 1: elif len(active_gripper_joint_names) == 1:
@ -269,7 +274,10 @@ class GripperCommandGroup(SimpleCommandGroup):
gripper_robotis_error = self.gripper_conversion.aperture_to_robotis(gripper_error) gripper_robotis_error = self.gripper_conversion.aperture_to_robotis(gripper_error)
elif (self.name == 'joint_gripper_finger_left') or (self.name == 'joint_gripper_finger_right'): elif (self.name == 'joint_gripper_finger_left') or (self.name == 'joint_gripper_finger_right'):
gripper_robotis_error = self.gripper_conversion.finger_to_robotis(gripper_error) gripper_robotis_error = self.gripper_conversion.finger_to_robotis(gripper_error)
robot.end_of_arm.move_by('stretch_gripper', gripper_robotis_error, v_r=self.goal['velocity'], a_r=self.goal['acceleration'])
robot.end_of_arm.move_by('stretch_gripper',
gripper_robotis_error,
v_r=self.goal['velocity'],
a_r=self.goal['acceleration'])
def update_execution(self, robot_status, **kwargs): def update_execution(self, robot_status, **kwargs):
self.error = None self.error = None
@ -311,9 +319,9 @@ class TelescopingCommandGroup(SimpleCommandGroup):
self.index = commanded_joint_names.index(self.name) self.index = commanded_joint_names.index(self.name)
self.active = True self.active = True
else: else:
err_str = 'Received a command for the wrist_extension joint and one or more telescoping_joints. \
These are mutually exclusive options. The joint names in the received command = \
{0}'.format(commanded_joint_names)
err_str = ("Received a command for the wrist_extension joint and one or more telescoping_joints. "
"These are mutually exclusive options. The joint names in the received command = "
"{0}").format(commanded_joint_names)
invalid_joints_callback(err_str) invalid_joints_callback(err_str)
return False return False
elif len(active_telescoping_joint_names) != 0: elif len(active_telescoping_joint_names) != 0:
@ -322,10 +330,10 @@ class TelescopingCommandGroup(SimpleCommandGroup):
self.is_telescoping = True self.is_telescoping = True
self.index = [commanded_joint_names.index(i) for i in self.telescoping_joints] self.index = [commanded_joint_names.index(i) for i in self.telescoping_joints]
else: else:
err_str = 'Commands with telescoping joints requires all telescoping joints to be present. \
Only received {0} of {1} telescoping joints. They are = \
{2}'.format(len(active_telescoping_joint_names), len(self.telescoping_joints),
active_telescoping_joint_names)
err_str = ("Commands with telescoping joints requires all telescoping joints to be present. "
"Only received {0} of {1} telescoping joints. They are = "
"{2}").format(len(active_telescoping_joint_names), len(self.telescoping_joints),
active_telescoping_joint_names)
invalid_joints_callback(err_str) invalid_joints_callback(err_str)
return False return False
@ -335,18 +343,28 @@ class TelescopingCommandGroup(SimpleCommandGroup):
self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None} self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
if self.active: if self.active:
if self.is_telescoping: if self.is_telescoping:
self.goal['position'] = sum([point.positions[i] for i in self.index]) if len(point.positions) > max(self.index) else None
self.goal['velocity'] = point.velocities[self.index[0]] if len(point.velocities) > self.index[0] else None
self.goal['acceleration'] = point.accelerations[self.index[0]] if len(point.accelerations) > self.index[0] else None
self.goal['contact_threshold'] = point.effort[self.index[0]] if len(point.effort) > self.index[0] else None
self.goal['position'] = sum([point.positions[i] for i in self.index]) \
if len(point.positions) > max(self.index) else None
self.goal['velocity'] = point.velocities[self.index[0]] \
if len(point.velocities) > self.index[0] else None
self.goal['acceleration'] = point.accelerations[self.index[0]] \
if len(point.accelerations) > self.index[0] else None
self.goal['contact_threshold'] = point.effort[self.index[0]] \
if len(point.effort) > self.index[0] else None
else: else:
self.goal['position'] = point.positions[self.index] if len(point.positions) > self.index else None
self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
self.goal['position'] = point.positions[self.index] \
if len(point.positions) > self.index else None
self.goal['velocity'] = point.velocities[self.index] \
if len(point.velocities) > self.index else None
self.goal['acceleration'] = point.accelerations[self.index] \
if len(point.accelerations) > self.index else None
self.goal['contact_threshold'] = point.effort[self.index] \
if len(point.effort) > self.index else None
if self.goal['position'] is None: if self.goal['position'] is None:
err_str = "Received goal point with positions array length={0}. This joint ({1})'s index is {2}. Length of array must cover all joints listed in commanded_joint_names.".format(len(point.positions), self.name, self.index)
err_str = ("Received goal point with positions array length={0}. "
"This joint ({1})'s index is {2}. Length of array must cover all joints listed "
"in commanded_joint_names.").format(len(point.positions), self.name, self.index)
invalid_goal_callback(err_str) invalid_goal_callback(err_str)
return False return False
@ -359,7 +377,8 @@ class TelescopingCommandGroup(SimpleCommandGroup):
v_m=self.goal['velocity'], v_m=self.goal['velocity'],
a_m=self.goal['acceleration'], a_m=self.goal['acceleration'],
contact_thresh_pos_N=self.goal['contact_threshold'], contact_thresh_pos_N=self.goal['contact_threshold'],
contact_thresh_neg_N=-self.goal['contact_threshold'] if self.goal['contact_threshold'] is not None else None)
contact_thresh_neg_N=-self.goal['contact_threshold'] \
if self.goal['contact_threshold'] is not None else None)
if extension_error_m < 0.0: if extension_error_m < 0.0:
kwargs['backlash_state']['wrist_extension_retracted'] = True kwargs['backlash_state']['wrist_extension_retracted'] = True
else: else:
@ -390,7 +409,8 @@ class LiftCommandGroup(SimpleCommandGroup):
v_m=self.goal['velocity'], v_m=self.goal['velocity'],
a_m=self.goal['acceleration'], a_m=self.goal['acceleration'],
contact_thresh_pos_N=self.goal['contact_threshold'], contact_thresh_pos_N=self.goal['contact_threshold'],
contact_thresh_neg_N=-self.goal['contact_threshold'] if self.goal['contact_threshold'] is not None else None)
contact_thresh_neg_N=-self.goal['contact_threshold'] \
if self.goal['contact_threshold'] is not None else None)
def update_execution(self, robot_status, **kwargs): def update_execution(self, robot_status, **kwargs):
self.error = None self.error = None
@ -403,7 +423,8 @@ class LiftCommandGroup(SimpleCommandGroup):
class MobileBaseCommandGroup(SimpleCommandGroup): class MobileBaseCommandGroup(SimpleCommandGroup):
def __init__(self): def __init__(self):
SimpleCommandGroup.__init__(self, 'joint_mobile_base_translation', (-0.5, 0.5), acceptable_joint_error=0.005)
SimpleCommandGroup.__init__(self, 'joint_mobile_base_translation', (-0.5, 0.5),
acceptable_joint_error=0.005)
self.incrementing_joint_names = ['translate_mobile_base', 'rotate_mobile_base'] self.incrementing_joint_names = ['translate_mobile_base', 'rotate_mobile_base']
self.active_translate_mobile_base = False self.active_translate_mobile_base = False
self.active_rotate_mobile_base = False self.active_rotate_mobile_base = False
@ -437,15 +458,15 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
self.active = True self.active = True
self.index = commanded_joint_names.index(self.name) self.index = commanded_joint_names.index(self.name)
else: else:
err_str = 'Received a command for the mobile base virtual joint ({0}}) and mobile base \
incremental motions ({1}). These are mutually exclusive options. \
The joint names in the received command = {2}'.format(self.name,
active_incrementing_joint_names, commanded_joint_names)
err_str = ("Received a command for the mobile base virtual joint ({0}}) "
"and mobile base incremental motions ({1}). These are "
"mutually exclusive options. The joint names in the received command = "
class="sa"> class="s2">" class="s2">{2} class="s2">").format(self.name, active_incrementing_joint_names, commanded_joint_names)
invalid_joints_callback(err_str) invalid_joints_callback(err_str)
return False return False
else: else:
err_str = 'Must be in manipulation mode to receive a command for the \
{0} joint. Current mode = {1}.'.format(self.name, robot_mode)
err_str = ("Must be in manipulation mode to receive a command for the "
"{0} joint. Current mode = {1}.").format(self.name, robot_mode)
invalid_joints_callback(err_str) invalid_joints_callback(err_str)
return False return False
elif len(active_incrementing_joint_names) != 0: elif len(active_incrementing_joint_names) != 0:
@ -458,8 +479,8 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
self.active_rotate_mobile_base = True self.active_rotate_mobile_base = True
self.index_rotate_mobile_base = commanded_joint_names.index('rotate_mobile_base') self.index_rotate_mobile_base = commanded_joint_names.index('rotate_mobile_base')
else: else:
err_str = 'Must be in position mode to receive a command for the {0} joint(s). \
Current mode = {1}.'.format(active_positioning_joint_names, robot_mode)
err_str = ("Must be in position mode to receive a command for the {0} joint(s). "
"Current mode = {1}.").format(active_positioning_joint_names, robot_mode)
invalid_joints_callback(err_str) invalid_joints_callback(err_str)
return False return False
@ -472,7 +493,12 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
if self.active: if self.active:
if self.active_translate_mobile_base or self.active_rotate_mobile_base: if self.active_translate_mobile_base or self.active_rotate_mobile_base:
if len(point.positions) <= self.index_translate_mobile_base and len(point.positions) <= self.index_rotate_mobile_base: if len(point.positions) <= self.index_translate_mobile_base and len(point.positions) <= self.index_rotate_mobile_base:
err_str = "Received goal point with positions array length={0}. These joints ({1})'s indices are {2} & {3} respectively. Length of array must cover all joints listed in commanded_joint_names.".format(len(point.positions), self.incrementing_joint_names, self.index_translate_mobile_base, self.index_rotate_mobile_base)
err_str = ("Received goal point with positions array length={0}. These joints ({1})'s "
"indices are {2} & {3} respectively. Length of array must cover all joints "
"listed in commanded_joint_names.").format(len(point.positions),
self.incrementing_joint_names,
self.index_translate_mobile_base,
self.index_rotate_mobile_base)
invalid_goal_callback(err_str) invalid_goal_callback(err_str)
return False return False
@ -492,16 +518,18 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
if (self.goal_translate_mobile_base['position'] is not None) and \ if (self.goal_translate_mobile_base['position'] is not None) and \
(self.goal_rotate_mobile_base['position'] is not None): (self.goal_rotate_mobile_base['position'] is not None):
err_str = 'Received a goal point with simultaneous translation and rotation mobile base goals. \
This is not allowed. Only one is allowed to be sent for a given goal point. \
translate_mobile_base = {0} and rotate_mobile_base = {1}'.format(self.goal_translate_mobile_base['position'],
self.goal_rotate_mobile_base['position'])
err_str = ("Received a goal point with simultaneous translation and rotation mobile base goals. "
"This is not allowed. Only one is allowed to be sent for a given goal point. "
"translate_mobile_base = {0} and rotate_mobile_base = {1}").format(self.goal_translate_mobile_base['position'],
self.goal_rotate_mobile_base['position'])
invalid_goal_callback(err_str) invalid_goal_callback(err_str)
return False return False
else: else:
goal_pos = point.positions[self.index] if len(point.positions) > self.index else None goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
if goal_pos is None: if goal_pos is None:
err_str = "Received goal point with positions array length={0}. This joint ({1})'s index is {2}. Length of array must cover all joints listed in commanded_joint_names.".format(len(point.positions), self.name, self.index)
err_str = ("Received goal point with positions array length={0}. This joint ({1})'s index "
"is {2}. Length of array must cover all joints listed in "
"commanded_joint_names.").format(len(point.positions), self.name, self.index)
invalid_goal_callback(err_str) invalid_goal_callback(err_str)
return False return False
@ -510,8 +538,8 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
if self.goal['position'] is None: if self.goal['position'] is None:
err_str = 'Received {0} goal point that is out of bounds. \
Range = {1}, but goal point = {2}.'.format(self.name, self.range, self.goal['position'])
err_str = ("Received {0} goal point that is out of bounds. "
"Range = {1}, but goal point = {2}.").format(self.name, self.range, self.goal['position'])
invalid_goal_callback(err_str) invalid_goal_callback(err_str)
return False return False

+ 19
- 21
stretch_core/nodes/joint_trajectory_server.py View File

@ -45,9 +45,8 @@ class JointTrajectoryAction:
# For now, ignore goal time and configuration tolerances. # For now, ignore goal time and configuration tolerances.
commanded_joint_names = goal.trajectory.joint_names commanded_joint_names = goal.trajectory.joint_names
rospy.loginfo('{0} joint_traj action: \
New trajectory received with joint_names = {1}'.format(self.node.node_name,
commanded_joint_names))
rospy.loginfo(("{0} joint_traj action: New trajectory received with joint_names = "
"{1}").format(self.node.node_name, commanded_joint_names))
################################################### ###################################################
# Decide what to do based on the commanded joints. # Decide what to do based on the commanded joints.
@ -70,15 +69,14 @@ class JointTrajectoryAction:
num_valid_points = sum([c.get_num_valid_commands() for c in command_groups]) num_valid_points = sum([c.get_num_valid_commands() for c in command_groups])
if num_valid_points <= 0: if num_valid_points <= 0:
err_str = 'Received a command without any valid joint names. \
Received joint names = {0}'.format(commanded_joint_names)
err_str = ("Received a command without any valid joint names."
"Received joint names = {0}").format(commanded_joint_names)
self.invalid_joints_callback(err_str) self.invalid_joints_callback(err_str)
self.node.robot_mode_rwlock.release_read() self.node.robot_mode_rwlock.release_read()
return return
elif num_valid_points != len(commanded_joint_names): elif num_valid_points != len(commanded_joint_names):
err_str = 'Received only {0} valid joints out of {1} total joints. \
Received joint names = {2}'.format(num_valid_points, len(commanded_joint_names),
commanded_joint_names)
err_str = ("Received only {0} valid joints out of {1} total joints. Received joint names = "
"{2}").format(num_valid_points, len(commanded_joint_names), commanded_joint_names)
self.invalid_joints_callback(err_str) self.invalid_joints_callback(err_str)
self.node.robot_mode_rwlock.release_read() self.node.robot_mode_rwlock.release_read()
return return
@ -87,8 +85,8 @@ class JointTrajectoryAction:
# Try to reach each of the goals in sequence until # Try to reach each of the goals in sequence until
# an error is detected or success is achieved. # an error is detected or success is achieved.
for pointi, point in enumerate(goal.trajectory.points): for pointi, point in enumerate(goal.trajectory.points):
rospy.logdebug('{0} joint_traj action: \
target point #{1} = <{2}>'.format(self.node.node_name, pointi, point))
rospy.logdebug(("{0} joint_traj action: "
"target point #{1} = <{2}>").format(self.node.node_name, pointi, point))
valid_goals = [c.set_goal(point, self.invalid_goal_callback, valid_goals = [c.set_goal(point, self.invalid_goal_callback,
manipulation_origin=self.node.mobile_base_manipulation_origin) manipulation_origin=self.node.mobile_base_manipulation_origin)
@ -111,8 +109,8 @@ class JointTrajectoryAction:
while not all(goals_reached): while not all(goals_reached):
if (rospy.Time.now() - goal_start_time) > self.node.default_goal_timeout_duration: if (rospy.Time.now() - goal_start_time) > self.node.default_goal_timeout_duration:
err_str = 'Time to execute the current goal point = <{0}> exceeded the \
default_goal_timeout = {1}'.format(point, self.node.default_goal_timeout_s)
err_str = ("Time to execute the current goal point = <{0}> exceeded the "
"default_goal_timeout = {1}").format(point, self.node.default_goal_timeout_s)
self.goal_tolerance_violated_callback(err_str) self.goal_tolerance_violated_callback(err_str)
self.node.robot_mode_rwlock.release_read() self.node.robot_mode_rwlock.release_read()
return return
@ -120,9 +118,9 @@ class JointTrajectoryAction:
# Check if a premption request has been received. # Check if a premption request has been received.
with self.node.robot_stop_lock: with self.node.robot_stop_lock:
if self.node.stop_the_robot or self.server.is_preempt_requested(): if self.node.stop_the_robot or self.server.is_preempt_requested():
rospy.logdebug('{0} joint_traj action: PREEMPTION REQUESTED, but not stopping \
current motions to allow smooth interpolation between \
old and new commands.'.format(self.node.node_name))
rospy.logdebug(("{0} joint_traj action: PREEMPTION REQUESTED, but not stopping "
"current motions to allow smooth interpolation between "
"old and new commands.").format(self.node.node_name))
self.server.set_preempted() self.server.set_preempted()
self.node.stop_the_robot = False self.node.stop_the_robot = False
self.node.robot_mode_rwlock.release_read() self.node.robot_mode_rwlock.release_read()
@ -135,7 +133,7 @@ class JointTrajectoryAction:
goals_reached = [c.goal_reached() for c in command_groups] goals_reached = [c.goal_reached() for c in command_groups]
update_rate.sleep() update_rate.sleep()
rospy.logdebug('{0} joint_traj action: Achieved target point.';.format(self.node.node_name))
rospy.logdebug("{0} joint_traj action: Achieved target point.";.format(self.node.node_name))
self.success_callback() self.success_callback()
self.node.robot_mode_rwlock.release_read() self.node.robot_mode_rwlock.release_read()
@ -143,21 +141,21 @@ class JointTrajectoryAction:
def invalid_joints_callback(self, err_str): def invalid_joints_callback(self, err_str):
if self.server.is_active() or self.server.is_preempt_requested(): if self.server.is_active() or self.server.is_preempt_requested():
rospy.logerr('{0} joint_traj action: {1}';.format(self.node.node_name, err_str))
rospy.logerr("{0} joint_traj action: {1}";.format(self.node.node_name, err_str))
self.result.error_code = self.result.INVALID_JOINTS self.result.error_code = self.result.INVALID_JOINTS
self.result.error_string = err_str self.result.error_string = err_str
self.server.set_aborted(self.result) self.server.set_aborted(self.result)
def invalid_goal_callback(self, err_str): def invalid_goal_callback(self, err_str):
if self.server.is_active() or self.server.is_preempt_requested(): if self.server.is_active() or self.server.is_preempt_requested():
rospy.logerr('{0} joint_traj action: {1}';.format(self.node.node_name, err_str))
rospy.logerr("{0} joint_traj action: {1}";.format(self.node.node_name, err_str))
self.result.error_code = self.result.INVALID_GOAL self.result.error_code = self.result.INVALID_GOAL
self.result.error_string = err_str self.result.error_string = err_str
self.server.set_aborted(self.result) self.server.set_aborted(self.result)
def goal_tolerance_violated_callback(self, err_str): def goal_tolerance_violated_callback(self, err_str):
if self.server.is_active() or self.server.is_preempt_requested(): if self.server.is_active() or self.server.is_preempt_requested():
rospy.logerr('{0} joint_traj action: {1}';.format(self.node.node_name, err_str))
rospy.logerr("{0} joint_traj action: {1}";.format(self.node.node_name, err_str))
self.result.error_code = self.result.GOAL_TOLERANCE_VIOLATED self.result.error_code = self.result.GOAL_TOLERANCE_VIOLATED
self.result.error_string = err_str self.result.error_string = err_str
self.server.set_aborted(self.result) self.server.set_aborted(self.result)
@ -177,7 +175,7 @@ class JointTrajectoryAction:
error_point.positions.append(clean_named_errors_dict[commanded_joint_name]) error_point.positions.append(clean_named_errors_dict[commanded_joint_name])
actual_point.positions.append(desired_point.positions[i] - clean_named_errors_dict[commanded_joint_name]) actual_point.positions.append(desired_point.positions[i] - clean_named_errors_dict[commanded_joint_name])
rospy.logdebug('{0} joint_traj action: sending feedback';.format(self.node.node_name))
rospy.logdebug("{0} joint_traj action: sending feedback";.format(self.node.node_name))
self.feedback.header.stamp = rospy.Time.now() self.feedback.header.stamp = rospy.Time.now()
self.feedback.joint_names = commanded_joint_names self.feedback.joint_names = commanded_joint_names
self.feedback.desired = desired_point self.feedback.desired = desired_point
@ -186,7 +184,7 @@ class JointTrajectoryAction:
self.server.publish_feedback(self.feedback) self.server.publish_feedback(self.feedback)
def success_callback(self): def success_callback(self):
rospy.loginfo('{0} joint_traj action: Achieved all target points!';.format(self.node.node_name))
rospy.loginfo("{0} joint_traj action: Achieved all target points!";.format(self.node.node_name))
self.result.error_code = self.result.SUCCESSFUL self.result.error_code = self.result.SUCCESSFUL
self.result.error_string = "Achieved all target points!" self.result.error_string = "Achieved all target points!"
self.server.set_succeeded(self.result) self.server.set_succeeded(self.result)

Write Preview
Loading…
Cancel
Save