From a3ae64545d287684bb2eca24d15daeaab57a4688 Mon Sep 17 00:00:00 2001 From: juschulz Date: Fri, 13 Dec 2024 10:59:48 +0100 Subject: [PATCH] [Receptionist] implemented feedback --- .../receptionist_clean_code.py | 10 +- src/pycram/ros_utils/robot_state_updater.py | 120 ------------------ src/pycram/utilities/robocup_utils.py | 15 --- 3 files changed, 4 insertions(+), 141 deletions(-) delete mode 100644 src/pycram/ros_utils/robot_state_updater.py diff --git a/demos/pycram_receptionist_demo/receptionist_clean_code.py b/demos/pycram_receptionist_demo/receptionist_clean_code.py index ce9ce9051..e1e8953cb 100644 --- a/demos/pycram_receptionist_demo/receptionist_clean_code.py +++ b/demos/pycram_receptionist_demo/receptionist_clean_code.py @@ -6,12 +6,13 @@ from pycram.designators.motion_designator import * from pycram.designators.object_designator import * from pycram.process_module import real_robot -from pycram.ros_utils.robot_state_updater import RobotStateUpdater +from pycram.ros_utils.object_state_updater import RobotStateUpdater from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher from pycram.utilities.robocup_utils import ImageSwitchPublisher from pycram.world_concepts.world_object import Object from pycram.worlds.bullet_world import BulletWorld import rospy +from pycrap import Robot # Initialize the Bullet world for simulation world = BulletWorld() @@ -20,17 +21,14 @@ v = VizMarkerPublisher() # Create and configure the robot object -robot = Object("hsrb", ObjectType.ROBOT, "../../resources/hsrb.urdf", pose=Pose([0, 0, 0])) +robot = Object("hsrb", Robot, "../../resources/hsrb.urdf", pose=Pose([0, 0, 0])) RobotStateUpdater("/tf", "/giskard_joint_states") image_switch_publisher = ImageSwitchPublisher() # Create environmental objects apartment = Object("kitchen", ObjectType.ENVIRONMENT, "suturo_lab_2.urdf") -# variables for communication with nlp -response = [None, None, None] -callback = False -pub_nlp = rospy.Publisher('/startListener', String, queue_size=16) +# class object for communication with nlp nlp = NLP_Functions() # Declare variables for humans diff --git a/src/pycram/ros_utils/robot_state_updater.py b/src/pycram/ros_utils/robot_state_updater.py deleted file mode 100644 index 72438cac9..000000000 --- a/src/pycram/ros_utils/robot_state_updater.py +++ /dev/null @@ -1,120 +0,0 @@ -import atexit -import tf - -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import JointState -from ..datastructures.world import World -from ..robot_description import RobotDescription -from ..datastructures.pose import Pose -from ..ros.data_types import Time -from ..ros.ros_tools import wait_for_message, create_timer, sleep - - -class RobotStateUpdater: - """ - Updates the robot in the World with information of the real robot published to ROS topics. - Infos used to update the robot are: - - * The current pose of the robot - * The current joint state of the robot - """ - - def __init__(self, tf_topic: str, joint_state_topic: str): - """ - The robot state updater uses a TF topic and a joint state topic to get the current state of the robot. - - :param tf_topic: Name of the TF topic, needs to publish geometry_msgs/TransformStamped - :param joint_state_topic: Name of the joint state topic, needs to publish sensor_msgs/JointState - """ - self.tf_listener = tf.TransformListener() - sleep(1.0) - self.tf_topic = tf_topic - self.joint_state_topic = joint_state_topic - - self.tf_timer = create_timer(0.1, self._subscribe_tf) - self.joint_state_timer = create_timer(0.1, self._subscribe_joint_state) - - atexit.register(self._stop_subscription) - - def _subscribe_tf(self, msg: TransformStamped) -> None: - """ - Callback for the TF timer, will do a lookup of the transform between map frame and the robot base frame. - - :param msg: TransformStamped message published to the topic - """ - trans, rot = self.tf_listener.lookupTransform("/map", RobotDescription.current_robot_description.base_link, Time(0)) - World.robot.set_pose(Pose(trans, rot)) - - def _subscribe_joint_state(self, msg: JointState) -> None: - """ - Sets the current joint configuration of the robot in the world to the configuration published on the - topic. Since this uses rospy.wait_for_message which can have errors when used with threads there might be an - attribute error in the rospy implementation. - - :param msg: JointState message published to the topic. - """ - try: - msg = wait_for_message(self.joint_state_topic, JointState) - for name, position in zip(msg.name, msg.position): - if name in list(World.robot.joints.keys()): - World.robot.set_joint_position(name, position) - except AttributeError: - pass - - def _stop_subscription(self) -> None: - """ - Stops the Timer for TF and joint states and therefore the updating of the robot in the world. - """ - self.tf_timer.shutdown() - self.joint_state_timer.shutdown() - - -class KitchenStateUpdater: - """ - Updates the robot in the Bullet World with information of the real robot published to ROS topics. - Infos used to update the robot are: - * The current pose of the robot - * The current joint state of the robot - """ - - def __init__(self, tf_topic: str, joint_state_topic: str): - """ - The robot state updater uses a TF topic and a joint state topic to get the current state of the robot. - - :param tf_topic: Name of the TF topic, needs to publish geometry_msgs/TransformStamped - :param joint_state_topic: Name of the joint state topic, needs to publish sensor_msgs/JointState - """ - self.tf_listener = tf.TransformListener() - sleep(1.0) - self.tf_topic = tf_topic - self.joint_state_topic = joint_state_topic - - self.joint_state_timer = create_timer(0.1, self._subscribe_joint_state) - - atexit.register(self._stop_subscription) - - def _subscribe_joint_state(self, msg: JointState) -> None: - """ - Sets the current joint configuration of the robot in the bullet world to the configuration published on the topic. - Since this uses rospy.wait_for_message which can have errors when used with threads there might be an attribute error - in the rospy implementation. - - :param msg: JointState message published to the topic. - """ - try: - msg = wait_for_message(self.joint_state_topic, JointState) - for name, position in zip(msg.name, msg.position): - try: - # Set the joint state if the joint exists - World.current_world.set_joint_state(name, position) - except KeyError: - # Handle the case where the joint name does not exist - pass - except AttributeError: - pass - - def _stop_subscription(self) -> None: - """ - Stops the Timer for TF and joint states and therefore the updating of the robot in the bullet world. - """ - self.joint_state_timer.shutdown() diff --git a/src/pycram/utilities/robocup_utils.py b/src/pycram/utilities/robocup_utils.py index b8eaebbe6..707c15030 100644 --- a/src/pycram/utilities/robocup_utils.py +++ b/src/pycram/utilities/robocup_utils.py @@ -212,21 +212,6 @@ def pub_now(self, image_id): rospy.loginfo("Image switch request published") -class HSRBMoveGripperReal(): - def __init__(self, latch=True): - self.pub = rospy.Publisher('/hsrb/gripper_controller/grasp/goal', GripperApplyEffortActionGoal, - queue_size=10, latch=latch) - self.msg = GripperApplyEffortActionGoal() - - def pub_now(self, motion: str): - if motion == "open": - self.msg.goal.effort = 0.8 - self.pub.publish(self.msg) - elif motion == "close": - self.msg.goal.effort = -0.8 - self.pub.publish(self.msg) - - class GraspListener: def __init__(self):