class PositionManager:
def __init__(self, position_dir):
self.fh = PositionFileHandler(position_dir)
self.manage_position_server = rospy.Service(
'/niryo_one/position/manage_position', ManagePosition,
self.callback_manage_position)
rospy.loginfo("service manage position created")
self.get_position_list_server = rospy.Service(
'/niryo_one/position/get_position_list', GetPositionList,
self.callback_get_position_list)
rospy.loginfo("get list position created")
self.validation = rospy.get_param(
"/niryo_one/robot_command_validation")
self.parameters_validation = ParametersValidation(self.validation)
def create_position_response(self, status, message, position=None):
position_msg = PositionMessage()
if position != None:
position_msg.name = position.name
position_msg.joints = position.joints
position_msg.rpy = Position.RPY(position.rpy.roll,
position.rpy.pitch,
position.rpy.yaw)
position_msg.point = Position.Point(position.point.x,
position.point.y,
position.point.z)
position_msg.quaternion = Position.Quaternion(
position.quaternion.x, position.quaternion.y,
position.quaternion.z, position.quaternion.w)
return {'status': status, 'message': message, 'position': position_msg}
def callback_manage_position(self, req):
cmd_type = req.cmd_type
position_name = req.position_name
position_msg = req.position
rpy = Position.RPY(position_msg.rpy.roll, position_msg.rpy.pitch,
position_msg.rpy.yaw)
point = Position.Point(position_msg.point.x, position_msg.point.y,
position_msg.point.z)
quaternion = Position.Quaternion(position_msg.quaternion.x,
position_msg.quaternion.y,
position_msg.quaternion.z,
position_msg.quaternion.w)
position_data = Position(name=position_msg.name,
joints=position_msg.joints,
rpy=rpy,
point=point,
quaternion=quaternion)
# GET an existing position
if cmd_type == PositionCommandType.GET:
pos = self.get_position(position_name)
if pos == None:
return self.create_position_response(
400, "No position found with name : " + str(position_name))
return self.create_position_response(200,
"Position has been found",
pos)
# CREATE new position
elif cmd_type == PositionCommandType.CREATE:
(new_position_name, msg) = self.create_new_position(position_data)
if new_position_name == None:
return self.create_position_response(400, msg)
new_position = self.get_position(new_position_name)
if new_position == None:
return self.create_position_response(
400, "Failed to create new position")
return self.create_position_response(200, msg, new_position)
# UPDATE existing sequence
elif cmd_type == PositionCommandType.UPDATE:
pos = self.get_position(position_name)
if pos == None:
return self.create_position_response(
400, "No position found with name : " + position_name)
(success, update_msg) = self.update_position(pos, position_data)
if not success:
return self.create_position_response(400, update_msg)
return self.create_position_response(200, update_msg, pos)
# DELETE sequence
elif cmd_type == PositionCommandType.DELETE:
success = self.delete_position(position_name)
if not success:
return self.create_position_response(
400,
"Could not delete position with name : " + position_name)
return self.create_position_response(200,
"Position has been deleted")
# Wrong cmd_type
else:
return self.create_sequence_response(400, "Wrong command type")
def delete_position(self, position_name):
try:
self.fh.remove_position(position_name)
except NiryoOneFileException as e:
return False
return True
def update_position(self, position, position_data):
try:
self.parameters_validation.validate_joints(position_data.joints)
except RobotCommanderException as e:
rospy.logwarn("Invalid joints value when updating position : " +
str(e.message))
return False, "Could not update position : " + str(e.message)
position.joints = position_data.joints
(position.point, position.rpy,
position.quaternion) = get_forward_kinematic(position.joints)
try:
self.fh.write_position(position)
except NiryoOneFileException as e:
return False, "Could not update position : " + str(e)
return True, "Position has been updated"
def get_position(self, position_name):
try:
return self.fh.read_position(position_name)
except NiryoOneFileException as e:
return None
def create_new_position(self, position):
position.name = position.name.rstrip()
if self.fh.check_position_name(position.name) == False:
return None, "Failed to create new position : position " + str(
position.name) + " already exists"
try:
self.parameters_validation.validate_joints(position.joints)
except RobotCommanderException as e:
rospy.logwarn("Invalid joints values when creating position : " +
str(e.message))
return None, "Failed to create new position : " + str(e.message)
try:
(position.point, position.rpy,
position.quaternion) = get_forward_kinematic(position.joints)
self.fh.write_position(position)
return position.name, "Position has been created"
except NiryoOneFileException as e:
return None, "Failed to create new position : " + str(e)
def callback_get_position_list(self, req=None):
pos_list = self.get_all_positions()
msg_list = []
for pos in pos_list:
position_msg = PositionMessage()
position_msg.name = pos.name
position_msg.joints = pos.joints
position_msg.rpy = Position.RPY(pos.rpy.roll, pos.rpy.pitch,
pos.rpy.yaw)
position_msg.point = Position.Point(pos.point.x, pos.point.y,
pos.point.z)
position_msg.quaternion = Position.Quaternion(
pos.quaternion.x, pos.quaternion.y, pos.quaternion.z,
pos.quaternion.w)
msg_list.append(position_msg)
return {'positions': msg_list}
def get_all_positions(self):
filenames = self.fh.get_all_filenames()
position_list = []
for f in filenames:
try:
position_name = self.fh.position_name_from_filename(f)
pos = self.get_position(position_name)
if pos != None:
position_list.append(pos)
except NiryoOneFileException as e:
pass
return position_list
class RobotCommander:
def compute_and_execute_plan(self):
plan = self.move_group_arm.compute_plan()
if not plan:
raise RobotCommanderException(
CommandStatus.PLAN_FAILED,
"Moveit failed to compute the plan.")
self.reset_controller()
rospy.loginfo("Send Moveit trajectory")
return self.arm_commander.execute_plan(plan)
def set_plan_and_execute(self, traj):
self.reset_controller()
rospy.loginfo("Send newly set trajectory to execute")
if traj == None:
raise RobotCommanderException(CommandStatus.PLAN_FAILED,
"Moveit failed to execute plan.")
return self.arm_commander.execute_plan(traj.trajectory)
def reset_controller(self):
msg = Empty()
self.reset_controller_pub.publish(msg)
def __init__(self, position_manager, trajectory_manager):
self.trajectory_manager = trajectory_manager
self.pos_manager = position_manager
moveit_commander.roscpp_initialize(sys.argv)
# Load all the sub-commanders
self.move_group_arm = MoveGroupArm()
self.arm_commander = ArmCommander(self.move_group_arm)
self.tool_commander = ToolCommander()
self.stop_trajectory_server = rospy.Service(
'niryo_one/commander/stop_command', SetBool,
self.callback_stop_command)
self.reset_controller_pub = rospy.Publisher(
'/niryo_one/steppers_reset_controller', Empty, queue_size=1)
# robot action server
self.server = actionlib.ActionServer(
'niryo_one/commander/robot_action',
RobotMoveAction,
self.on_goal,
self.on_cancel,
auto_start=False)
self.current_goal_handle = None
self.learning_mode_on = False
self.joystick_enabled = False
self.is_active_server = rospy.Service('niryo_one/commander/is_active',
GetInt, self.callback_is_active)
self.learning_mode_subscriber = rospy.Subscriber(
'/niryo_one/learning_mode', Bool, self.callback_learning_mode)
self.joystick_enabled_subscriber = rospy.Subscriber(
'/niryo_one/joystick_interface/is_enabled', Bool,
self.callback_joystick_enabled)
self.validation = rospy.get_param(
"/niryo_one/robot_command_validation")
self.parameters_validation = ParametersValidation(self.validation)
def set_saved_position(self, cmd):
rospy.loginfo("set saved position")
pos = self.pos_manager.get_position(cmd.saved_position_name)
self.arm_commander.set_joint_target(pos.joints)
def set_saved_trajectory(self, cmd):
traj = self.trajectory_manager.get_trajectory(cmd.saved_trajectory_id)
return self.set_plan_and_execute(traj.trajectory_plan)
def execute_command(self, cmd):
cmd_type = cmd.cmd_type
status = CommandStatus.ROS_ERROR
message = "Unknown problem occured during command execution"
if cmd_type == CommandType.TOOL:
self.tool_commander.send_tool_command(cmd.tool_cmd)
status = CommandStatus.SUCCESS # todo get status from send_tool_command
message = "Tool command has been successfully processed"
else: # move command
if cmd_type == CommandType.EXECUTE_TRAJ:
status, message = self.set_plan_and_execute(cmd.Trajectory)
elif cmd_type == CommandType.SAVED_TRAJECTORY:
status, message = self.set_saved_trajectory(cmd)
else:
if cmd_type == CommandType.JOINTS:
self.arm_commander.set_joint_target(cmd.joints)
elif cmd_type == CommandType.POSE:
self.arm_commander.set_pose_target(
cmd.position.x, cmd.position.y, cmd.position.z,
cmd.rpy.roll, cmd.rpy.pitch, cmd.rpy.yaw)
elif cmd_type == CommandType.POSITION:
self.arm_commander.set_position_target(
cmd.position.x, cmd.position.y, cmd.position.z)
elif cmd_type == CommandType.RPY:
self.arm_commander.set_rpy_target(cmd.rpy.roll,
cmd.rpy.pitch,
cmd.rpy.yaw)
elif cmd_type == CommandType.SHIFT_POSE:
self.arm_commander.set_shift_pose_target(
cmd.shift.axis_number, cmd.shift.value)
elif cmd_type == CommandType.POSE_QUAT:
self.arm_commander.set_pose_quat_target(cmd.pose_quat)
elif cmd_type == CommandType.SAVED_POSITION:
self.set_saved_position(cmd)
status, message = self.compute_and_execute_plan()
return (status, message)
def cancel_command(self):
self.arm_commander.stop_current_plan()
self.tool_commander.stop_tool_command(
) # todo handle goal cancelation for tools (client side)
def callback_stop_command(self, req):
self.cancel_command()
return True, "Command stopped"
# robot action server functions
# Check if no other command is being processed
# - Validate params
# - Execute command and return status + message
# - Cancel command if asked
def start(self):
self.server.start()
rospy.loginfo("Action Server started (Robot Commander)")
def create_result(self, status, message):
result = RobotMoveResult()
result.status = status
result.message = message
return result
def callback_learning_mode(self, msg):
activate = msg.data
if not self.learning_mode_on and activate:
self.cancel_current_command()
self.learning_mode_on = activate
def callback_joystick_enabled(self, msg):
self.joystick_enabled = msg.data
def callback_is_active(self, req):
if self.current_goal_handle is not None:
return 1
return 0
def on_goal(self, goal_handle):
rospy.loginfo("Robot Action Server - Received goal. Check if exists")
# Check if learning mode ON
if self.learning_mode_on:
result = self.create_result(
CommandStatus.LEARNING_MODE_ON,
"You need to deactivate learning mode to execute a new command"
)
goal_handle.set_rejected(result)
return
# Check if joystick enabled
if self.joystick_enabled:
result = self.create_result(
CommandStatus.JOYSTICK_ENABLED,
"You need to deactivate joystick to execute a new command")
goal_handle.set_rejected(result)
return
# check if still have a goal -> set_rejected()
if self.current_goal_handle is not None:
result = self.create_result(
CommandStatus.GOAL_STILL_ACTIVE,
"Current command is still active. Cancel it if you want to execute a new one"
)
goal_handle.set_rejected(result)
return
# validate parameters -> set_rejected (msg : validation or commander error)
try:
rospy.loginfo("Robot Acton Sever - checking paramter Validity")
self.validate_params(goal_handle.goal.goal.cmd)
except RobotCommanderException as e:
result = self.create_result(e.status, e.message)
goal_handle.set_rejected(result)
rospy.loginfo("Robot Action Server - Invalid parameters")
return
# set accepted
self.current_goal_handle = goal_handle
self.current_goal_handle.set_accepted()
rospy.loginfo("Robot Action Server - Goal has been accepted")
# Launch compute + execution in a new thread
w = threading.Thread(name="worker", target=self.execute_command_action)
w.start()
rospy.loginfo(
"Robot Action Server : Executing command in a new thread")
def on_cancel(self, goal_handle):
rospy.loginfo("Received cancel command")
if goal_handle == self.current_goal_handle:
self.cancel_current_command()
else:
rospy.loginfo(
"Robot Action Server - No current goal, nothing to do")
def execute_command_action(self):
cmd = self.current_goal_handle.goal.goal.cmd
rospy.loginfo("passing to executing command")
result = self.create_result(CommandStatus.ROS_ERROR,
"error with executing command")
response = None
try:
(status, message) = self.execute_command(cmd)
response = self.create_result(status, message)
result = response
except RobotCommanderException as e:
result = self.create_result(e.status, e.message)
rospy.loginfo("An exception was thrown during command execution")
if not response:
self.current_goal_handle.set_aborted(result)
rospy.loginfo("Execution has been aborted")
elif response.status == CommandStatus.SUCCESS:
self.current_goal_handle.set_succeeded(result)
rospy.loginfo("Goal has been set as succeeded")
elif response.status == CommandStatus.STOPPED:
self.current_goal_handle.set_canceled(result)
rospy.loginfo("Goal has been successfully canceled")
else:
self.current_goal_handle.set_aborted(result)
rospy.loginfo("Unknown result, goal has been set as aborted")
self.current_goal_handle = None
# Send a cancel signal to Moveit interface
def cancel_current_command(self):
try:
self.cancel_command()
except RobotCommanderException:
rospy.logwarn("Could not cancel current command ")
# command validation
def validate_params(self, cmd):
cmd_type = cmd.cmd_type
if cmd_type == CommandType.JOINTS:
self.parameters_validation.validate_joints(cmd.joints)
elif cmd_type == CommandType.POSE:
self.parameters_validation.validate_position(cmd.position)
self.parameters_validation.validate_orientation(cmd.rpy)
elif cmd_type == CommandType.POSITION:
self.parameters_validation.validate_position(cmd.position)
elif cmd_type == CommandType.RPY:
self.parameters_validation.validate_orientation(cmd.rpy)
elif cmd_type == CommandType.SHIFT_POSE:
self.parameters_validation.validate_shift_pose(cmd.shift)
elif cmd_type == CommandType.EXECUTE_TRAJ:
self.parameters_validation.validate_trajectory(cmd.Trajectory)
elif cmd_type == CommandType.TOOL:
self.parameters_validation.validate_tool_command(cmd.tool_cmd)
elif cmd_type == CommandType.POSE_QUAT:
self.parameters_validation.validate_position(
cmd.pose_quat.position)
self.parameters_validation.validate_orientation_quaternion(
cmd.pose_quat.orientation)
elif cmd_type == CommandType.SAVED_POSITION:
self.validate_saved_position(cmd.saved_position_name)
elif CommandType.SAVED_TRAJECTORY:
self.validate_saved_trajectory(cmd)
else:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"Wrong command type")
def validate_saved_trajectory(self, cmd):
rospy.loginfo("Checking saved trajectory validity")
saved_traj = self.trajectory_manager.get_trajectory(
cmd.saved_trajectory_id)
if saved_traj == None:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"Saved trajectory not found")
self.parameters_validation.validate_trajectory(
saved_traj.trajectory_plan)
def validate_saved_position(self, position_name):
rospy.loginfo("Checking joints validity")
saved_position = self.pos_manager.get_position(position_name)
if saved_position == None:
raise RobotCommanderException(CommandStatus.INVALID_PARAMETERS,
"Saved position not found")
self.parameters_validation.validate_joints(saved_position.joints)