Skip to content

Commit

Permalink
[MultiverseAction] DetectionAction is now working in multiverse.
Browse files Browse the repository at this point in the history
  • Loading branch information
AbdelrhmanBassiouny committed Aug 7, 2024
1 parent 44f9b61 commit 4736b31
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 3 deletions.
3 changes: 1 addition & 2 deletions src/pycram/process_modules/pr2_process_modules.py
Original file line number Diff line number Diff line change
Expand Up @@ -87,14 +87,13 @@ def _execute(self, desig: DetectingMotion):
robot = World.robot
object_type = desig.object_type
# Should be "wide_stereo_optical_frame"
cam_frame_name = RobotDescription.current_robot_description.get_camera_frame()
camera_link_name = RobotDescription.current_robot_description.get_camera_link()
# should be [0, 0, 1]
camera_description = RobotDescription.current_robot_description.cameras[
list(RobotDescription.current_robot_description.cameras.keys())[0]]
front_facing_axis = camera_description.front_facing_axis

objects = World.current_world.get_object_by_type(object_type)
camera_link_name = [link.name for link in robot.links.values() if link.tf_frame == cam_frame_name][0]
for obj in objects:
if btr.visible(obj, robot.get_link_pose(camera_link_name), front_facing_axis):
return obj
Expand Down
6 changes: 5 additions & 1 deletion src/pycram/world_concepts/world_object.py
Original file line number Diff line number Diff line change
Expand Up @@ -814,7 +814,11 @@ def joint_states(self, joint_states: Dict[int, JointState]) -> None:
:param joint_states: A dictionary with the joint id as key and the current state of the joint as value.
"""
for joint in self.joints.values():
joint.current_state = joint_states[joint.id]
if joint.name not in self.robot_virtual_move_base_joints_names():
joint.current_state = joint_states[joint.id]

def robot_virtual_move_base_joints_names(self):
return self.robot_description.virtual_move_base_joints.names

def remove_saved_states(self) -> None:
"""
Expand Down

0 comments on commit 4736b31

Please sign in to comment.