From 89886154a75dbd378d1a53b8e80382933c5af639 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Wed, 27 Nov 2024 13:12:41 +0100 Subject: [PATCH 1/5] [environment_joint_state] added joint state updater for environment --- ...tate_updater.py => joint_state_updater.py} | 59 ++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) rename src/pycram/ros_utils/{robot_state_updater.py => joint_state_updater.py} (52%) diff --git a/src/pycram/ros_utils/robot_state_updater.py b/src/pycram/ros_utils/joint_state_updater.py similarity index 52% rename from src/pycram/ros_utils/robot_state_updater.py rename to src/pycram/ros_utils/joint_state_updater.py index 314d85535..f64774961 100644 --- a/src/pycram/ros_utils/robot_state_updater.py +++ b/src/pycram/ros_utils/joint_state_updater.py @@ -1,6 +1,8 @@ import atexit + +import rospy import tf -import time +import time from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState @@ -67,3 +69,58 @@ def _stop_subscription(self) -> None: """ self.tf_timer.shutdown() self.joint_state_timer.shutdown() + + +class EnvironmentStateUpdater: + """ + Updates the environment in the Bullet World with information of the real environment published to ROS topics. + Infos used to update the envi are: + * The current pose of the environment + * The current joint state of the environment + """ + + def __init__(self, tf_topic: str, joint_state_topic: str): + """ + The environment state updater uses a TF topic and a joint state topic to get the current state of the environment. + + :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() + rospy.sleep(1) + self.tf_topic = tf_topic + self.joint_state_topic = joint_state_topic + + self.joint_state_timer = rospy.Timer(rospy.Duration(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 environment 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 = rospy.wait_for_message(self.joint_state_topic, JointState) + for name, position in zip(msg.name, msg.position): + try: + # Attempt to get the joint state. This might throw a KeyError if the joint name doesn't exist + if World.environment.get_joint_state(name) is None: + continue + # Set the joint state if the joint exists + World.environment.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 environment in the bullet world. + """ + self.joint_state_timer.shutdown() + self.joint_state_timer.shutdown() From b67c8a2cfead50f09c105dd2dee59a729dea5e6e Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Wed, 27 Nov 2024 13:40:52 +0100 Subject: [PATCH 2/5] [environment_joint_state] use new robot description --- src/pycram/ros_utils/joint_state_updater.py | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/pycram/ros_utils/joint_state_updater.py b/src/pycram/ros_utils/joint_state_updater.py index f64774961..bbc8f2051 100644 --- a/src/pycram/ros_utils/joint_state_updater.py +++ b/src/pycram/ros_utils/joint_state_updater.py @@ -7,10 +7,11 @@ from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState from ..datastructures.world import World -from ..robot_descriptions import robot_description from ..datastructures.pose import Pose +from ..robot_description import RobotDescription from ..ros.data_types import Time, Duration from ..ros.ros_tools import wait_for_message, create_timer +# robot_description.base_frame, class RobotStateUpdater: @@ -34,8 +35,8 @@ def __init__(self, tf_topic: str, joint_state_topic: str): self.tf_topic = tf_topic self.joint_state_topic = joint_state_topic - self.tf_timer = create_timer(Duration().from_sec(0.1), self._subscribe_tf) - self.joint_state_timer = create_timer(Duration().from_sec(0.1), self._subscribe_joint_state) + self.tf_timer = rospy.Timer(rospy.Duration.from_sec(0.1), self._subscribe_tf) + self.joint_state_timer = rospy.Timer(rospy.Duration.from_sec(0.1), self._subscribe_joint_state) atexit.register(self._stop_subscription) @@ -45,7 +46,7 @@ def _subscribe_tf(self, msg: TransformStamped) -> None: :param msg: TransformStamped message published to the topic """ - trans, rot = self.tf_listener.lookupTransform("/map", robot_description.base_frame, Time(0)) + trans, rot = self.tf_listener.lookupTransform("/map", RobotDescription.current_robot_description.base_link, rospy.Time(0)) World.robot.set_pose(Pose(trans, rot)) def _subscribe_joint_state(self, msg: JointState) -> None: @@ -57,9 +58,17 @@ def _subscribe_joint_state(self, msg: JointState) -> None: :param msg: JointState message published to the topic. """ try: - msg = wait_for_message(self.joint_state_topic, JointState) + msg = rospy.wait_for_message(self.joint_state_topic, JointState) for name, position in zip(msg.name, msg.position): - World.robot.set_joint_position(name, position) + try: + # Attempt to get the joint state. This might throw a KeyError if the joint name doesn't exist + if World.robot.get_joint_state(name) is None: + continue + # Set the joint state if the joint exists + World.robot.set_joint_position(name, position) + except KeyError: + # Handle the case where the joint name does not exist + pass except AttributeError: pass From b87c4a527c06ab657cc95ab4d1f4b64769997024 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Wed, 27 Nov 2024 14:01:32 +0100 Subject: [PATCH 3/5] [joint_state_updater] fixed for duration --- src/pycram/ros/ros_tools.py | 7 ++++--- src/pycram/ros_utils/joint_state_updater.py | 22 ++++++--------------- 2 files changed, 10 insertions(+), 19 deletions(-) diff --git a/src/pycram/ros/ros_tools.py b/src/pycram/ros/ros_tools.py index 96bcf7ce6..8dd23ec62 100644 --- a/src/pycram/ros/ros_tools.py +++ b/src/pycram/ros/ros_tools.py @@ -30,8 +30,8 @@ def get_parameter(name: str) -> Any: return rospy.get_param(name) -def wait_for_message(topic_name: str): - return rospy.wait_for_message(topic_name) +def wait_for_message(topic_name: str, msg_type: Any): + return rospy.wait_for_message(topic_name, msg_type) def is_master_online(): @@ -41,5 +41,6 @@ def is_master_online(): def sleep(duration: float): rospy.sleep(duration) + def create_timer(duration: int, callback, oneshot=False): - return rospy.Timer(rospy.Duration(duration), callback, oneshot=oneshot) \ No newline at end of file + return rospy.Timer(duration, callback, oneshot=oneshot) diff --git a/src/pycram/ros_utils/joint_state_updater.py b/src/pycram/ros_utils/joint_state_updater.py index bbc8f2051..259351770 100644 --- a/src/pycram/ros_utils/joint_state_updater.py +++ b/src/pycram/ros_utils/joint_state_updater.py @@ -7,11 +7,10 @@ from geometry_msgs.msg import TransformStamped from sensor_msgs.msg import JointState from ..datastructures.world import World -from ..datastructures.pose import Pose from ..robot_description import RobotDescription +from ..datastructures.pose import Pose from ..ros.data_types import Time, Duration from ..ros.ros_tools import wait_for_message, create_timer -# robot_description.base_frame, class RobotStateUpdater: @@ -34,9 +33,8 @@ def __init__(self, tf_topic: str, joint_state_topic: str): time.sleep(1) self.tf_topic = tf_topic self.joint_state_topic = joint_state_topic - - self.tf_timer = rospy.Timer(rospy.Duration.from_sec(0.1), self._subscribe_tf) - self.joint_state_timer = rospy.Timer(rospy.Duration.from_sec(0.1), self._subscribe_joint_state) + self.tf_timer = create_timer(Duration().from_sec(0.1), self._subscribe_tf) + self.joint_state_timer = create_timer(Duration().from_sec(0.1), self._subscribe_joint_state) atexit.register(self._stop_subscription) @@ -46,7 +44,7 @@ def _subscribe_tf(self, msg: TransformStamped) -> None: :param msg: TransformStamped message published to the topic """ - trans, rot = self.tf_listener.lookupTransform("/map", RobotDescription.current_robot_description.base_link, rospy.Time(0)) + 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: @@ -58,17 +56,9 @@ def _subscribe_joint_state(self, msg: JointState) -> None: :param msg: JointState message published to the topic. """ try: - msg = rospy.wait_for_message(self.joint_state_topic, JointState) + msg = wait_for_message(self.joint_state_topic, JointState) for name, position in zip(msg.name, msg.position): - try: - # Attempt to get the joint state. This might throw a KeyError if the joint name doesn't exist - if World.robot.get_joint_state(name) is None: - continue - # Set the joint state if the joint exists - World.robot.set_joint_position(name, position) - except KeyError: - # Handle the case where the joint name does not exist - pass + World.robot.set_joint_position(name, position) except AttributeError: pass From d25c2facdd371235eeda44ac71ea64affce5f7a8 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Thu, 28 Nov 2024 11:57:19 +0100 Subject: [PATCH 4/5] [state updater] moved state publisher --- .../{joint_state_updater.py => object_state_updater.py} | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) rename src/pycram/ros_utils/{joint_state_updater.py => object_state_updater.py} (95%) diff --git a/src/pycram/ros_utils/joint_state_updater.py b/src/pycram/ros_utils/object_state_updater.py similarity index 95% rename from src/pycram/ros_utils/joint_state_updater.py rename to src/pycram/ros_utils/object_state_updater.py index 259351770..6a158f12f 100644 --- a/src/pycram/ros_utils/joint_state_updater.py +++ b/src/pycram/ros_utils/object_state_updater.py @@ -72,7 +72,7 @@ def _stop_subscription(self) -> None: class EnvironmentStateUpdater: """ - Updates the environment in the Bullet World with information of the real environment published to ROS topics. + Updates the environment in the World with information of the real environment published to ROS topics. Infos used to update the envi are: * The current pose of the environment * The current joint state of the environment @@ -96,7 +96,7 @@ def __init__(self, tf_topic: str, joint_state_topic: str): def _subscribe_joint_state(self, msg: JointState) -> None: """ - Sets the current joint configuration of the environment in the bullet world to the configuration published on the topic. + Sets the current joint configuration of the environment 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. @@ -119,7 +119,7 @@ def _subscribe_joint_state(self, msg: JointState) -> None: def _stop_subscription(self) -> None: """ - Stops the Timer for TF and joint states and therefore the updating of the environment in the bullet world. + Stops the Timer for TF and joint states and therefore the updating of the environment in the world. """ self.joint_state_timer.shutdown() self.joint_state_timer.shutdown() From 78b087d43ff784e2fc42541384f588c692e19ba9 Mon Sep 17 00:00:00 2001 From: Vanessa Hassouna Date: Thu, 28 Nov 2024 12:14:55 +0100 Subject: [PATCH 5/5] [state updater] doc note its only usable with real robot --- src/pycram/ros_utils/object_state_updater.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/pycram/ros_utils/object_state_updater.py b/src/pycram/ros_utils/object_state_updater.py index 6a158f12f..cccb72ac8 100644 --- a/src/pycram/ros_utils/object_state_updater.py +++ b/src/pycram/ros_utils/object_state_updater.py @@ -20,6 +20,8 @@ class RobotStateUpdater: * The current pose of the robot * The current joint state of the robot + .. Note:: This class can only be used if the topics are present in the RSO network on the real robot/world, + hence it is not testable in the CI. """ def __init__(self, tf_topic: str, joint_state_topic: str): @@ -76,6 +78,9 @@ class EnvironmentStateUpdater: Infos used to update the envi are: * The current pose of the environment * The current joint state of the environment + + .. Note:: This class can only be used if the topics are present in the RSO network on the real robot/world, + hence it is not testable in the CI. """ def __init__(self, tf_topic: str, joint_state_topic: str):