Fix ROS-only code in Robot

Author: gonzalocasas

src/compas_fab/backends/ros/planner_backend_moveit.py

@@ -93,13 +93,18 @@ class MoveItPlanner(PlannerBackend):
                                             GetPlanningSceneResponse)
 
     def init_planner(self):
-        self.collision_object_topic = Topic(self, '/collision_object',
-                                            'moveit_msgs/CollisionObject', queue_size=None)
+        self.collision_object_topic = Topic(
+            self,
+            '/collision_object',
+            'moveit_msgs/CollisionObject',
+            queue_size=None)
         self.collision_object_topic.advertise()
 
         self.attached_collision_object_topic = Topic(
-            self, '/attached_collision_object',
-            'moveit_msgs/AttachedCollisionObject', queue_size=None)
+            self,
+            '/attached_collision_object',
+            'moveit_msgs/AttachedCollisionObject',
+            queue_size=None)
         self.attached_collision_object_topic.advertise()
 
     def dispose_planner(self):
@@ -133,7 +138,10 @@ def inverse_kinematics_async(self, callback, errback, frame, base_link, group,
                                        avoid_collisions=avoid_collisions,
                                        attempts=attempts)
 
-        self.GET_POSITION_IK(self, (ik_request, ), callback, errback)
+        def convert_to_positions(response):
+            callback(response.solution.joint_state.position)
+
+        self.GET_POSITION_IK(self, (ik_request, ), convert_to_positions, errback)
 
     def forward_kinematics_async(self, callback, errback, joint_positions, base_link,
                                  group, joint_names, ee_link):
@@ -145,8 +153,11 @@ def forward_kinematics_async(self, callback, errback, joint_positions, base_link
         robot_state = RobotState(
             joint_state, MultiDOFJointState(header=header))
 
+        def convert_to_frame(response):
+            callback(response.pose_stamped[0].pose.frame)
+
         self.GET_POSITION_FK(self, (header, fk_link_names,
-                                    robot_state), callback, errback)
+                                    robot_state), convert_to_frame, errback)
 
     def plan_cartesian_motion_async(self, callback, errback, frames, base_link,
                                     ee_link, group, joint_names, joint_types,

src/compas_fab/robots/robot.py

@@ -833,12 +833,10 @@ def inverse_kinematics(self, frame_WCF, start_configuration=None,
         frame_RCF = self.represent_frame_in_RCF(frame_WCF, group)
         frame_RCF.point /= self.scale_factor  # must be in meters
 
-        response = self.client.inverse_kinematics(frame_RCF, base_link,
+        joint_positions = self.client.inverse_kinematics(frame_RCF, base_link,
                                                   group, joint_names, joint_positions,
                                                   avoid_collisions, constraints, attempts,
                                                   attached_collision_meshes)
-
-        joint_positions = response.solution.joint_state.position
         joint_positions = self._scale_joint_values(joint_positions, self.scale_factor)
         # full configuration # TODO group config?
         configuration = Configuration(joint_positions, self.get_configurable_joint_types())
@@ -900,8 +898,7 @@ def forward_kinematics(self, configuration, group=None, backend=None, link_name=
 
         if not backend:
             if self.client:
-                response = self.client.forward_kinematics(full_joint_positions, base_link_name, group, full_joint_names, link_name)
-                frame_RCF = response.pose_stamped[0].pose.frame
+                frame_RCF = self.client.forward_kinematics(full_joint_positions, base_link_name, group, full_joint_names, link_name)
                 frame_RCF.point *= self.scale_factor
             else:
                 frame_WCF = self.model.forward_kinematics(group_joint_state, link_name)