def validate_shift_pose(self, shift):
if (shift.axis_number not in [0, 1, 2, 3, 4, 5]):
raise RobotCommanderException(
CommandStatus.INVALID_PARAMETERS,
"shift axis number not in [0,1,2,3,4,5]")
if (shift.value == 0 or shift.value < -1.0 or shift.value > 1.0):
raise RobotCommanderException(
CommandStatus.INVALID_PARAMETERS,
"shift value can't be null and < -1 or > 1")
def execute_plan(self, plan, wait=False):
if plan:
# reset event
self.traj_finished_event.clear()
self.current_goal_id = None
self.current_goal_result = GoalStatus.LOST
# send traj and wait
self.move_group_arm.execute(plan, wait=False)
trajectory_time_out = 1.5 * self.get_plan_time(plan)
# if trajectory_time_out is less than to seconds, the default value will be 2 seconds
if trajectory_time_out < TrajectoryTimeOutMin:
trajectory_time_out = TrajectoryTimeOutMin
if self.traj_finished_event.wait(trajectory_time_out):
plan = None
if self.current_goal_result == GoalStatus.SUCCEEDED:
self.gauss_ros_logger.publish_log_status(
"INFO",
"ArmCommander Command has been successfully processed")
return CommandStatus.SUCCESS, "Command has been successfully processed"
elif self.current_goal_result == GoalStatus.PREEMPTED:
self.gauss_ros_logger.publish_log_status(
"WARNING",
"ArmCommander Command has been successfully stopped")
return CommandStatus.STOPPED, "Command has been successfully stopped"
elif self.current_goal_result == GoalStatus.ABORTED:
# if joint_trajectory_controller aborts the goal, it will still try to
# finish executing the trajectory --> so we ask it to stop from here
self.set_position_hold_mode()
self.gauss_ros_logger.publish_log_status(
"WARNING", "ArmCommander Command has been aborted")
return CommandStatus.CONTROLLER_PROBLEMS, "Command has been aborted"
else: # what else could happen ?
self.gauss_ros_logger.publish_log_status(
"ERROR",
"Unknown error, try to restart, or contact the support to know more"
)
return CommandStatus.ROS_ERROR, "Unknown error, try to restart, or contact the support to know more"
else:
# todo cancel goal
plan = None
self.gauss_ros_logger.publish_log_status(
"ERROR",
"Trajectory timeout - Try to resend the command or restart the robot"
)
raise RobotCommanderException(
CommandStatus.CONTROLLER_PROBLEMS,
"Trajectory timeout - Try to resend the command or restart the robot"
)
else:
raise RobotCommanderException(
CommandStatus.NO_PLAN_AVAILABLE,
"You are trying to execute a plan which does't exist")
def validate_orientation(self, orientation):
v = self.validation['rpy_limits']
if (orientation.roll < v['roll']['min'] or orientation.roll > v['roll']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"rot. x (roll) not in range ( " + str(v['roll']['min']) + " , " + str(v['roll']['max']) + " )")
if (orientation.pitch < v['pitch']['min'] or orientation.pitch > v['pitch']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"rot.y (pitch) not in range ( " + str(v['pitch']['min']) + " , " + str(v['pitch']['max']) + " )")
if (orientation.yaw < v['yaw']['min'] or orientation.yaw > v['yaw']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"rot.z (yaw) not in range ( " + str(v['yaw']['min']) + " , " + str(v['yaw']['max']) + " )")
def validate_position(self, position):
v = self.validation['position_limits']
if (position.x < v['x']['min'] or position.x > v['x']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"x not in range ( " + str(v['x']['min']) + " , " + str(v['x']['max']) + " )")
if (position.y < v['y']['min'] or position.y > v['y']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"y not in range ( " + str(v['y']['min']) + " , " + str(v['y']['max']) + " )")
if (position.z < v['z']['min'] or position.z > v['z']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"z not in range ( " + str(v['z']['min']) + " , " + str(v['z']['max']) + " )")
def send_tool_command(self, cmd):
goal = self.create_goal(cmd)
self.action_client.send_goal(goal)
rospy.loginfo("Tool command sent")
self.gauss_ros_logger.publish_log_status(
"INFO", "ToolCommander Tool command sent")
# wait for goal transition to DONE
self.action_client.wait_for_result()
# if goal has been rejected/aborted, stop tracking it and return error
if self.has_problem():
self.gauss_ros_logger.publish_log_status(
"ERROR", "ToolCommander Tool command sent has_problem")
message = self.action_client.get_result().message
self.action_client.stop_tracking_goal()
self.gauss_ros_logger.publish_log_status("ERROR", message)
raise RobotCommanderException(CommandStatus.TOOL_FAILED, message)
def validate_joints(self, joint_array):
v = self.validation['joint_limits']
if len(joint_array) != 6:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS, "Joint array must have 6 joints")
if( joint_array[0] < v['j1']['min'] or joint_array[0] > v['j1']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"joint 1 not in range ( " + str(v['j1']['min']) + " , " + str(v['j1']['max']) + " )")
if( joint_array[1] < v['j2']['min'] or joint_array[1] > v['j2']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"joint 2 not in range ( " + str(v['j2']['min']) + " , " + str(v['j2']['max']) + " )")
if( joint_array[2] < v['j3']['min'] or joint_array[2] > v['j3']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"joint 3 not in range ( " + str(v['j3']['min']) + " , " + str(v['j3']['max']) + " )")
if( joint_array[3] < v['j4']['min'] or joint_array[3] > v['j4']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"joint 4 not in range ( " + str(v['j4']['min']) + " , " + str(v['j4']['max']) + " )")
if( joint_array[4] < v['j5']['min'] or joint_array[4] > v['j5']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"joint 5 not in range ( " + str(v['j5']['min']) + " , " + str(v['j5']['max']) + " )")
if( joint_array[5] < v['j6']['min'] or joint_array[5] > v['j6']['max']):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"joint 6 not in range ( " + str(v['j6']['min']) + " , " + str(v['j6']['max']) + " )")
def set_position_target(self, x, y, z):
try:
self.move_group_arm.set_position_target(x, y, z)
except Exception, e:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
str(e))
def set_joint_target(self, joint_array):
try:
self.move_group_arm.set_joint_value_target(joint_array)
except Exception, e:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
str(e))
def get_plan_time(self, plan):
if plan:
return plan.joint_trajectory.points[-1].time_from_start.to_sec()
else:
raise RobotCommanderException(CommandStatus.NO_PLAN_AVAILABLE,
"No current plan found")
def set_shift_pose_target(self, axis_number, value):
try:
self.move_group_arm.set_shift_pose_target(axis_number, value)
except Exception, e:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
str(e))
def set_pose_quat_target(self, pose_msg):
try:
self.move_group_arm.set_pose_quat_target(pose_msg)
except Exception, e:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
str(e))
def set_rpy_target(self, roll, pitch, yaw):
try:
self.move_group_arm.set_rpy_target(roll, pitch, yaw)
except Exception, e:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
str(e))
def validate_orientation_quaternion(self, quat):
norm = quat.x * quat.x + quat.y * quat.y + quat.z * quat.z + quat.w * quat.w
norm = sqrt(norm)
if (abs(norm - 1.0) > 0.001):
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"Quaternian is not normalised.")