From 45952ce87969f47ce506f35f48cd83da18b60d53 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Thu, 28 Sep 2023 23:25:32 -0700 Subject: [PATCH] Add HelloNode.get_joint_state() + other improvements The get_joint_state() method makes it easier to get information about a joint, including whether the joint is moving. A mode variable is added to make it easier to determine what mode the driver is in. Lastly, some of HelloNode's variables are made private to make it easier to use the class from iPython. --- hello_helpers/src/hello_helpers/hello_misc.py | 37 ++++++++++++++----- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 459f7a8..acbf645 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -18,7 +18,7 @@ from control_msgs.msg import FollowJointTrajectoryAction from control_msgs.msg import FollowJointTrajectoryGoal from trajectory_msgs.msg import JointTrajectoryPoint import tf2_ros -from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointCloud2, JointState from std_srvs.srv import Trigger, TriggerRequest from std_msgs.msg import String @@ -68,9 +68,10 @@ def get_left_finger_state(joint_states): class HelloNode: def __init__(self): - self.joint_state = None + self.joint_states = None self.point_cloud = None self.tool = None + self.mode = None self.dryrun = False @classmethod @@ -79,15 +80,18 @@ class HelloNode: i.main(name, name, wait_for_first_pointcloud) return i - def joint_states_callback(self, joint_state): - self.joint_state = joint_state + def _joint_states_callback(self, joint_states): + self.joint_states = joint_states - def point_cloud_callback(self, point_cloud): + def _point_cloud_callback(self, point_cloud): self.point_cloud = point_cloud - def tool_callback(self, tool_string): + def _tool_callback(self, tool_string): self.tool = tool_string.data + def _mode_callback(self, mode_string): + self.mode = mode_string.data + def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False): if self.dryrun: return @@ -201,6 +205,18 @@ class HelloNode: assert(self.tool is not None) return self.tool + def get_mode(self): + assert(self.mode is not None) + return self.mode + + def get_joint_state(self, joint_name, moving_threshold=0.001): + i = self.joint_states.name.index(joint_name) + joint_position = self.joint_states.position[i] + joint_velocity = self.joint_states.velocity[i] + joint_effort = self.joint_states.effort[i] + joint_is_moving = abs(joint_velocity) > moving_threshold + return (joint_position, joint_velocity, joint_effort, joint_is_moving) + def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True): rospy.init_node(node_name) self.node_name = rospy.get_name() @@ -211,13 +227,16 @@ class HelloNode: if not server_reached: rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.') sys.exit() - + + self._joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self._joint_states_callback) + self.tf2_buffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) - self.tool_subscriber = rospy.Subscriber('/tool', String, self.tool_callback) + self._tool_subscriber = rospy.Subscriber('/tool', String, self._tool_callback) + self._mode_subscriber = rospy.Subscriber('/mode', String, self._mode_callback) - self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback) + self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self._point_cloud_callback) self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1) rospy.wait_for_service('/home_the_robot')