Merge pull request #157 from SemRoCo/set_odom_without_group_name

made group_name in set_odometry optional

Author: ichumuh

src/giskardpy/goals/joint_goals.py

@@ -49,9 +49,10 @@ def __init__(self,
 
 
 class SetOdometry(NonMotionGoal):
+    odom_joints = (OmniDrive, DiffDrive, OmniDrivePR22)
     def __init__(self,
-                 group_name: str,
                  base_pose: PoseStamped,
+                 group_name: Optional[str] = None,
                  name: Optional[str] = None,
                  start_condition: cas.Expression = cas.TrueSymbol,
                  hold_condition: cas.Expression = cas.FalseSymbol,
@@ -62,10 +63,19 @@ def __init__(self,
         super().__init__(name)
         if god_map.is_goal_msg_type_execute() and not god_map.is_standalone():
             raise GoalInitalizationException(f'It is not allowed to combine {str(self)} with plan and execute.')
-        brumbrum_joint_name = god_map.world.groups[group_name].root_link.child_joint_names[0]
-        brumbrum_joint = god_map.world.joints[brumbrum_joint_name]
-        if not isinstance(brumbrum_joint, (OmniDrive, DiffDrive, OmniDrivePR22)):
-            raise GoalInitalizationException(f'Group {group_name} has no odometry joint.')
+        if self.group_name is None:
+            drive_joints = god_map.world.search_for_joint_of_type(self.odom_joints)
+            if len(drive_joints) == 0:
+                raise GoalInitalizationException('No drive joints in world')
+            elif len(drive_joints) == 1:
+                brumbrum_joint = drive_joints[0]
+            else:
+                raise GoalInitalizationException('Multiple drive joint found in world, please set \'group_name\'')
+        else:
+            brumbrum_joint_name = god_map.world.groups[group_name].root_link.child_joint_names[0]
+            brumbrum_joint = god_map.world.joints[brumbrum_joint_name]
+            if not isinstance(brumbrum_joint, self.odom_joints):
+                raise GoalInitalizationException(f'Group {self.group_name} has no odometry joint.')
         base_pose = transform_msg(brumbrum_joint.parent_link_name, base_pose).pose
         god_map.world.state[brumbrum_joint.x.name].position = base_pose.position.x
         god_map.world.state[brumbrum_joint.y.name].position = base_pose.position.y

test/test_integration_pr2.py

@@ -1098,6 +1098,22 @@ def test_SetSeedConfiguration(self, zero_pose: PR2TestWrapper):
         zero_pose.set_joint_goal(zero_pose.default_pose)
         zero_pose.plan()
 
+    def test_SetOdometry(self, zero_pose: PR2TestWrapper):
+        pose = PoseStamped()
+        pose.header.frame_id = 'map'
+        pose.pose.position.x = 1
+        pose.pose.orientation.w = 1
+        zero_pose.set_seed_odometry(base_pose=pose)
+        zero_pose.set_joint_goal(zero_pose.better_pose)
+        zero_pose.plan()
+        pose = PoseStamped()
+        pose.header.frame_id = 'map'
+        pose.pose.position.x = 1
+        pose.pose.orientation.w = 1
+        zero_pose.set_seed_odometry(base_pose=pose, group_name=zero_pose.robot_name)
+        zero_pose.set_joint_goal(zero_pose.better_pose)
+        zero_pose.plan()
+
     def test_drive_into_apartment(self, apartment_setup: PR2TestWrapper):
         base_pose = PoseStamped()
         base_pose.header.frame_id = 'base_footprint'

test/utils_for_tests.py

@@ -252,10 +252,8 @@ def has_odometry_joint(self, group_name: Optional[str] = None):
         return isinstance(joint, (OmniDrive, DiffDrive))
 
     def set_seed_odometry(self, base_pose, group_name: Optional[str] = None):
-        if group_name is None:
-            group_name = self.robot_name
-        self.motion_goals.set_seed_odometry(group_name=group_name,
-                                            base_pose=base_pose)
+        self.motion_goals.set_seed_odometry(base_pose=base_pose,
+                                            group_name=group_name)
 
     def set_localization(self, map_T_odom: PoseStamped):
         if self.set_localization_srv is not None: