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')