Merge branch 'develop' into 'master'

Add frames in workspace

See merge request niryo/niryo-one-s/ned_ros_stack!300

niryo_robot_poses_handlers/msg/DynamicFrame.msg

@@ -1,6 +1,7 @@
 string name # maximum lenght of dynamic_frame's name is 30 characters
 string new_name
 string description
+bool belong_to_workspace
 
 geometry_msgs/Point position
 niryo_robot_msgs/RPY rpy

niryo_robot_poses_handlers/scripts/poses_handlers_node.py

@@ -54,23 +54,6 @@ def __init__(self):
         self.__tool_id = 0
         rospy.Subscriber('/niryo_robot_tools_commander/current_id', Int32, self.__callback_tool_id)
 
-        # Workspaces
-        ws_dir = rospy.get_param("~workspace_dir")
-
-        self.__ws_manager = WorkspaceManager(ws_dir)
-        rospy.Service('~manage_workspace', ManageWorkspace, self.__callback_manage_workspace)
-        rospy.Service('~get_workspace_ratio', GetWorkspaceRatio, self.__callback_workspace_ratio)
-        rospy.Service('~get_workspace_list', GetNameDescriptionList, self.__callback_workspace_list)
-        rospy.Service('~get_workspace_poses', GetWorkspaceRobotPoses, self.__callback_get_workspace_poses)
-        rospy.Service('~get_workspace_points', GetWorkspacePoints, self.__callback_get_workspace_points)
-        rospy.Service('~get_workspace_matrix_poses', GetWorkspaceMatrixPoses, self.__callback_get_workspace_matrix)
-
-        if rospy.has_param('~gazebo_workspaces'):
-            for ws_name, ws_poses in rospy.get_param('~gazebo_workspaces').items():
-                if ws_name not in self.get_available_workspaces()[0]:
-                    rospy.loginfo("Poses Handler - Adding the {} workspace...".format(ws_name))
-                    self.create_workspace_from_points(ws_name, "", [Point(*point) for point in ws_poses])
-
         # Grips
         tool_config_dict = rospy.get_param("niryo_robot_tools_commander/tool_list", dict())
         self.__tool_id_gripname_dict = {tool["id"]: "default_" + tool["name"].replace(" ", "_")
@@ -108,6 +91,23 @@ def __init__(self):
         # Publisher dynamic frames
         thread.start_new_thread(self.dynamic_frame_manager.publish_frames, ())
 
+        # Workspaces
+        ws_dir = rospy.get_param("~workspace_dir")
+
+        self.__ws_manager = WorkspaceManager(ws_dir)
+        rospy.Service('~manage_workspace', ManageWorkspace, self.__callback_manage_workspace)
+        rospy.Service('~get_workspace_ratio', GetWorkspaceRatio, self.__callback_workspace_ratio)
+        rospy.Service('~get_workspace_list', GetNameDescriptionList, self.__callback_workspace_list)
+        rospy.Service('~get_workspace_poses', GetWorkspaceRobotPoses, self.__callback_get_workspace_poses)
+        rospy.Service('~get_workspace_points', GetWorkspacePoints, self.__callback_get_workspace_points)
+        rospy.Service('~get_workspace_matrix_poses', GetWorkspaceMatrixPoses, self.__callback_get_workspace_matrix)
+
+        if rospy.has_param('~gazebo_workspaces'):
+            for ws_name, ws_poses in rospy.get_param('~gazebo_workspaces').items():
+                if ws_name not in self.get_available_workspaces()[0]:
+                    rospy.loginfo("Poses Handler - Adding the {} workspace...".format(ws_name))
+                    self.create_workspace_from_points(ws_name, "", [Point(*point) for point in ws_poses])
+
         # Set a bool to mentioned this node is initialized
         rospy.set_param('~initialized', True)
 
@@ -249,7 +249,8 @@ def __callback_manage_dynamic_frame(self, req):
                 return CommandStatus.DYNAMIC_FRAME_CREATION_FAILED, "Frame have 3 points, {} given".format(
                     len(frame.poses))
             try:
-                self.create_dynamic_frame_from_poses(frame.name, frame.description, frame.poses)
+                self.create_dynamic_frame_from_poses(frame.name, frame.description, frame.poses,
+                                                     frame.belong_to_workspace)
                 return CommandStatus.SUCCESS, "Created dynamic frame '{}'".format(frame.name)
             except NiryoRobotFileException as e:
                 return CommandStatus.FILE_ALREADY_EXISTS, str(e)
@@ -260,15 +261,16 @@ def __callback_manage_dynamic_frame(self, req):
                 return CommandStatus.DYNAMIC_FRAME_CREATION_FAILED, "Frame have 3 points, {} given".format(
                     len(frame.points))
             try:
-                self.create_dynamic_frame_from_points(frame.name, frame.description, frame.points)
+                self.create_dynamic_frame_from_points(frame.name, frame.description, frame.points,
+                                                      frame.belong_to_workspace)
                 return CommandStatus.SUCCESS, "Created dynamic frame '{}'".format(frame.name)
             except NiryoRobotFileException as e:
                 return CommandStatus.FILE_ALREADY_EXISTS, str(e)
             except (ValueError, Exception) as e:
                 return CommandStatus.POSES_HANDLER_CREATION_FAILED, str(e)
         elif cmd == req.DELETE:
             try:
-                self.remove_dynamic_frame(frame.name)
+                self.remove_dynamic_frame(frame.name, frame.belong_to_workspace)
                 return CommandStatus.SUCCESS, "Removed dynamic frame '{}'".format(frame.name)
             except Exception as e:
                 return CommandStatus.POSES_HANDLER_REMOVAL_FAILED, str(e)
@@ -330,8 +332,12 @@ def create_workspace(self, name, description, robot_poses):
             pose_raw = [[pose.position.x, pose.position.y, pose.position.z],
                         [pose.rpy.roll, pose.rpy.pitch, pose.rpy.yaw]]
             robot_poses_raw.append(pose_raw)
+
         self.__ws_manager.create(name, points, robot_poses_raw, description)
 
+        # Asssociate frame
+        self.dynamic_frame_manager.create(name, [points[0]] + [points[3]] + [points[1]], description, True)
+
     def create_workspace_from_points(self, name, description, points):
         """
 
@@ -344,13 +350,18 @@ def create_workspace_from_points(self, name, description, points):
             points_raw.append([point.x, point.y, point.z])
         self.__ws_manager.create(name, points_raw, description)
 
+        # Asssociate frame
+        self.dynamic_frame_manager.create(name, [points_raw[0]] + [points_raw[3]] + [points_raw[1]], description, True)
+
     def remove_workspace(self, name):
         """
         Removes a workspace
         :param name: name of the workspace to remove
         """
         self.__ws_manager.remove(name)
 
+        self.dynamic_frame_manager.remove(name, True)
+
     def get_available_workspaces(self):
         """
         Returns a list of all available workspace names.
@@ -483,7 +494,7 @@ def get_available_poses(self):
         return self.__pos_manager.get_all_names_w_description()
 
     # Dynamic frame
-    def create_dynamic_frame_from_poses(self, name, description, poses):
+    def create_dynamic_frame_from_poses(self, name, description, poses, belong_to_workspace=False):
         """
         Create a dynamic frame from ManageDynamicFrame message fields
 
@@ -503,9 +514,9 @@ def create_dynamic_frame_from_poses(self, name, description, poses):
             rospy.loginfo("Tip point\n{}".format(point))
             points.append([point.x, point.y, point.z])
 
-        self.dynamic_frame_manager.create(name, points, description)
+        self.dynamic_frame_manager.create(name, points, description, belong_to_workspace)
 
-    def create_dynamic_frame_from_points(self, name, description, points):
+    def create_dynamic_frame_from_points(self, name, description, points, belong_to_workspace=False):
         """
         Create a dynamic frame from ManageDynamicFrame message fields
 
@@ -521,17 +532,17 @@ def create_dynamic_frame_from_points(self, name, description, points):
         for point in points:
             points_raw.append([point.x, point.y, point.z])
 
-        self.dynamic_frame_manager.create(name, points_raw, description)
+        self.dynamic_frame_manager.create(name, points_raw, description, belong_to_workspace)
 
-    def remove_dynamic_frame(self, name):
+    def remove_dynamic_frame(self, name, belong_to_workspace=False):
         """
         Asks dynamic frame manager to remove dynamic frame
 
         :param name: dynamic frame name
         :type name: str
         :return: None
         """
-        self.dynamic_frame_manager.remove(name)
+        self.dynamic_frame_manager.remove(name, belong_to_workspace)
 
     def edit_dynamic_frame(self, name, new_name, description):
         """

niryo_robot_poses_handlers/src/niryo_robot_poses_handlers/dynamic_frame_manager.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python
 
+from asyncore import read
 from niryo_robot_poses_handlers.file_manager import FileManager, NiryoRobotFileException
 from niryo_robot_poses_handlers.transform_functions import euler_from_matrix, quaternion_from_euler
 
@@ -17,6 +18,7 @@ class DynamicFrame(object):
     def __init__(self, name, description):
         self.name = name
         self.description = description
+        self.belong_to_workspace = False
         self.robot_poses = []
         self.points = []
         self.static_transform_stamped = []
@@ -25,6 +27,7 @@ def to_dict(self):
         dict_ = dict()
         dict_["name"] = self.name
         dict_["description"] = self.description
+        dict_["belong_to_workspace"] = self.belong_to_workspace
         dict_["points"] = self.points
         dict_["static_transform_stamped"] = self.static_transform_stamped
         return dict_
@@ -33,6 +36,7 @@ def to_dict(self):
     def from_dict(cls, dict_):
         dyn_frame = cls(dict_["name"], dict_["description"])
         dyn_frame.points = dict_["points"]
+        dyn_frame.belong_to_workspace = dict_["belong_to_workspace"]
         dyn_frame.static_transform_stamped = dict_["static_transform_stamped"]
 
         return dyn_frame
@@ -92,10 +96,11 @@ def calculate_transform(points):
 
         return point_o, point_vx, point_vy, q
 
-    def create(self, frame_name, points, description=""):
+    def create(self, frame_name, points, description="", belong_to_workspace=False):
         """
         Create a new local frame
         """
+        print(points)
         if self.exists(frame_name):
             raise NiryoRobotFileException("Frame {} already exists".format(frame_name))
 
@@ -115,6 +120,7 @@ def create(self, frame_name, points, description=""):
 
         dynamic_frame = DynamicFrame(frame_name, description)
         dynamic_frame.points = points
+        dynamic_frame.belong_to_workspace = belong_to_workspace
         dynamic_frame.static_transform_stamped = [[point_o.x, point_o.y, point_o.z], q.tolist()]
 
         self._write(frame_name, dynamic_frame)
@@ -148,6 +154,7 @@ def edit_frame(self, frame_name, new_frame_name, description=""):
 
         dynamic_frame = DynamicFrame(new_frame_name, description)
         dynamic_frame.points = points
+        dynamic_frame.belong_to_workspace = dynamic_frame.belong_to_workspace
         dynamic_frame.static_transform_stamped = [[point_o.x, point_o.y, point_o.z], q.tolist()]
 
         self._write(new_frame_name, dynamic_frame)
@@ -156,10 +163,16 @@ def edit_frame(self, frame_name, new_frame_name, description=""):
         if (frame_name != new_frame_name):
             FileManager.remove(self, frame_name)
 
-    def remove(self, frame_name):
+    def remove(self, frame_name, belong_to_workspace=False):
         """
         Remove a frame
         """
+        if not belong_to_workspace:
+            frame = self.read(frame_name)
+            if frame.belong_to_workspace:
+                raise NiryoRobotFileException("Frame {} can't be removed because it belong to a workspace".format(
+                    frame_name))
+
         if not self.exists(frame_name):
             raise NiryoRobotFileException("Frame {} not already exists".format(frame_name))
 

niryo_robot_python_ros_wrapper/src/niryo_robot_python_ros_wrapper/ros_wrapper.py

@@ -1044,7 +1044,7 @@ def clean_trajectory_memory(self):
 
     # - Dynamic frame
 
-    def save_dynamic_frame_from_poses(self, frame_name, description, list_robot_poses):
+    def save_dynamic_frame_from_poses(self, frame_name, description, list_robot_poses, belong_to_workspace=False):
         """
         Create a dynamic frame with 3 poses (origin, x, y)
 
@@ -1054,6 +1054,8 @@ def save_dynamic_frame_from_poses(self, frame_name, description, list_robot_pose
         :type description: str
         :param list_robot_poses: 3 poses needed to create the frame
         :type list_robot_poses: list[list[float]]
+        :param belong_to_workspace: indicate if the frame belong to a workspace
+        :type belong_to_workspace: boolean
         :return: status, message
         :rtype: (int, str)
         """
@@ -1066,14 +1068,15 @@ def save_dynamic_frame_from_poses(self, frame_name, description, list_robot_pose
         req.dynamic_frame.name = frame_name
         req.dynamic_frame.description = description
         req.dynamic_frame.poses = list_poses
+        req.dynamic_frame.belong_to_workspace = belong_to_workspace
 
         result = self._call_service('/niryo_robot_poses_handlers/manage_dynamic_frame', ManageDynamicFrame, req)
 
         self._check_result_status(result)
 
         return self._classic_return_w_check(result)
 
-    def save_dynamic_frame_from_points(self, frame_name, description, list_points):
+    def save_dynamic_frame_from_points(self, frame_name, description, list_points, belong_to_workspace=False):
         """
         Create a dynamic frame with 3 points (origin, x, y)
 
@@ -1083,6 +1086,8 @@ def save_dynamic_frame_from_points(self, frame_name, description, list_points):
         :type description: str
         :param list_points: 3 points needed to create the frame
         :type list_points: list[list[float]]
+        :param belong_to_workspace: indicate if the frame belong to a workspace
+        :type belong_to_workspace: boolean
         :return: status, message
         :rtype: (int, str)
         """
@@ -1095,6 +1100,7 @@ def save_dynamic_frame_from_points(self, frame_name, description, list_points):
         req.dynamic_frame.name = frame_name
         req.dynamic_frame.description = description
         req.dynamic_frame.points = points
+        req.dynamic_frame.belong_to_workspace = belong_to_workspace
 
         result = self._call_service('/niryo_robot_poses_handlers/manage_dynamic_frame', ManageDynamicFrame, req)
 
@@ -1129,12 +1135,14 @@ def edit_dynamic_frame(self, frame_name, new_frame_name, new_description):
 
         return self._classic_return_w_check(result)
 
-    def delete_dynamic_frame(self, frame_name):
+    def delete_dynamic_frame(self, frame_name, belong_to_workspace=False):
         """
         Delete a dynamic frame
 
         :param frame_name: name of the frame to remove
         :type frame_name: str
+        :param belong_to_workspace: indicate if the frame belong to a workspace
+        :type belong_to_workspace: boolean
         :return: status, message
         :rtype: (int, str)
         """
@@ -1143,6 +1151,7 @@ def delete_dynamic_frame(self, frame_name):
         req = ManageDynamicFrameRequest()
         req.cmd = ManageDynamicFrameRequest.DELETE
         req.dynamic_frame.name = frame_name
+        req.dynamic_frame.belong_to_workspace = belong_to_workspace
 
         result = self._call_service('/niryo_robot_poses_handlers/manage_dynamic_frame', ManageDynamicFrame, req)
 

niryo_robot_user_interface/src/niryo_robot_user_interface/tcp_server/command_interpreter.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python
 import rospy
-
 from enum import Enum
 from niryo_robot_python_ros_wrapper.ros_wrapper_enums import ShiftPose, PinMode, PinState, PinID, \
     ToolID, ConveyorID, ConveyorDirection, CommandEnum, ConveyorTTL, ConveyorCan
@@ -553,32 +552,38 @@ def __get_saved_dynamic_frame(self, *param_list):
         frame = self.__niryo_robot.get_saved_dynamic_frame(*param_list)
         return self.__send_answer(*frame)
 
-    @check_nb_args(5)
+    @check_nb_args(6)
     def __save_dynamic_frame_from_poses(self, *param_list):
         name = param_list[0]
         description = param_list[1]
-        list_poses = list(param_list[2:])
+        list_poses = list(param_list[2:-1])
+        belong_to_workspace = bool(param_list[-1])
 
-        self.__niryo_robot.save_dynamic_frame_from_poses(name, description, list_poses)
+        print(belong_to_workspace)
+
+        self.__niryo_robot.save_dynamic_frame_from_poses(name, description, list_poses, belong_to_workspace)
         return self.__send_answer()
 
-    @check_nb_args(5)
+    @check_nb_args(6)
     def __save_dynamic_frame_from_points(self, *param_list):
         name = param_list[0]
         description = param_list[1]
-        list_poses = list(param_list[2:])
+        list_poses = list(param_list[2:-1])
+        belong_to_workspace = bool(param_list[-1])
 
-        self.__niryo_robot.save_dynamic_frame_from_points(name, description, list_poses)
+        self.__niryo_robot.save_dynamic_frame_from_points(name, description, list_poses, belong_to_workspace)
         return self.__send_answer()
 
     @check_nb_args(3)
     def __edit_dynamic_frame(self, *param_list):
         self.__niryo_robot.edit_dynamic_frame(*param_list)
         return self.__send_answer()
 
-    @check_nb_args(1)
+    @check_nb_args(2)
     def __delete_dynamic_frame(self, *param_list):
-        self.__niryo_robot.delete_dynamic_frame(*param_list)
+        name = param_list[0]
+        belong_to_workspace = bool(param_list[1])
+        self.__niryo_robot.delete_dynamic_frame(name, belong_to_workspace)
         return self.__send_answer()
 
     @check_nb_args(2)