From f8ae48b7296fffcc39b116782a87074a566c26fa Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Sun, 8 Dec 2024 12:54:00 +0100 Subject: [PATCH 1/2] [vizmarker] msgs --- src/pycram/ros_utils/viz_marker_publisher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/ros_utils/viz_marker_publisher.py b/src/pycram/ros_utils/viz_marker_publisher.py index 3454759b8..280605257 100644 --- a/src/pycram/ros_utils/viz_marker_publisher.py +++ b/src/pycram/ros_utils/viz_marker_publisher.py @@ -245,7 +245,7 @@ def _make_marker_array(self, name, marker_type: int, marker_pose: Pose, marker_s new_marker.id = self.current_id new_marker.header.frame_id = frame_id new_marker.ns = name - new_marker.header.stamp = Time.now() + # new_marker.header.stamp = Time.now() new_marker.type = marker_type new_marker.action = Marker.ADD new_marker.pose = marker_pose.pose From fe3edd8107add69ea2dbda7a1e5ccb148dce50c3 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Sun, 8 Dec 2024 12:52:56 +0100 Subject: [PATCH 2/2] [pr2procecssmodule] adjustments for real robot eg move_base for nav instead of giskard Gripper open and close with enums and bug in calculation for head --- .../process_modules/pr2_process_modules.py | 23 ++++++++----------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index e849566b7..7a7a991b3 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -8,6 +8,7 @@ import numpy as np from .. import world_reasoning as btr +from ..external_interfaces.move_base import query_pose_nav from ..process_module import ProcessModule, ProcessModuleManager from ..external_interfaces.ik import request_ik from ..ros.logging import logdebug @@ -21,9 +22,10 @@ from ..datastructures.world import World from ..world_concepts.world_object import Object from ..datastructures.pose import Pose -from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, MovementType +from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, MovementType, GripperState from ..external_interfaces import giskard from ..external_interfaces.robokudo import * +from ..ros.logging import loginfo, logwarn, logdebug if TYPE_CHECKING: from ..designators.object_designator import ObjectDesignatorDescription @@ -31,7 +33,7 @@ try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 except ImportError: - pass + logdebug("Pr2GripperCommandGoal not found") class Pr2Navigation(ProcessModule): @@ -189,13 +191,8 @@ class Pr2NavigationReal(ProcessModule): """ def _execute(self, designator: MoveMotion) -> Any: - logdebug(f"Sending goal to giskard to Move the robot") - if designator.keep_joint_states: - joint_positions = World.current_world.robot.get_positions_of_controllable_joints() - giskard.set_joint_goal(joint_positions) - giskard.achieve_cartesian_goal(designator.target, RobotDescription.current_robot_description.base_link, "map", - allow_gripper_collision_=False, - use_monitor=World.current_world.conf.use_giskard_monitor) + logdebug(f"Sending goal to movebase to Move the robot") + query_pose_nav(designator.target) class Pr2MoveHeadReal(ProcessModule): @@ -213,7 +210,7 @@ def _execute(self, desig: LookingMotion): pose_in_tilt = local_transformer.transform_pose(target, robot.get_link_tf_frame("head_tilt_link")) new_pan = np.arctan2(pose_in_pan.position.y, pose_in_pan.position.x) - new_tilt = np.arctan2(pose_in_tilt.position.z, pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2) * -1 + new_tilt = np.arctan2(pose_in_tilt.position.z, np.sqrt(pose_in_tilt.position.x ** 2 + pose_in_tilt.position.y ** 2)) * -1 current_pan = robot.get_joint_position("head_pan_joint") current_tilt = robot.get_joint_position("head_tilt_joint") @@ -276,15 +273,15 @@ def activate_callback(): loginfo("Started gripper Movement") def done_callback(state, result): - loginfo(f"Reached goal {designator.motion}: {result.reached_goal}") + loginfo(f"Reached goal {designator.motion}") def feedback_callback(msg): pass goal = Pr2GripperCommandGoal() - goal.command.position = 0.0 if designator.motion == "close" else 0.1 + goal.command.position = 0.0 if designator.motion == GripperState.CLOSE else 0.1 goal.command.max_effort = 50.0 - if designator.gripper == "right": + if designator.gripper == Arms.RIGHT: controller_topic = "r_gripper_controller/gripper_action" else: controller_topic = "l_gripper_controller/gripper_action"