From 3c84e5accee337f3a86323cc7d931b4c0e6bbae4 Mon Sep 17 00:00:00 2001 From: Jennipher Day Date: Wed, 8 Jul 2026 15:14:34 -0400 Subject: [PATCH 1/7] ros topic discovery in ophyd device --- src/bluesky_ros/ophyd_draft.py | 56 ++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 src/bluesky_ros/ophyd_draft.py diff --git a/src/bluesky_ros/ophyd_draft.py b/src/bluesky_ros/ophyd_draft.py new file mode 100644 index 00000000..e64eb617 --- /dev/null +++ b/src/bluesky_ros/ophyd_draft.py @@ -0,0 +1,56 @@ +from ophyd import Device, Component as Cpt, EpicsSignal, EpicsSignalRO +import rclpy +from rclpy.node import Node +import time + +""" +My first draft of an ophyd device yay! +""" + +class ROS_Node(Node): + def __init__(self): + super().__init__("arm_node") + + def list_topics(self): + return self.get_topic_names_and_types() + +class Joint(Device): + def __init__(self, device, **kwargs): + super().__init__(device, **kwargs) + + # ros topic - can you see it, use rclpy to list it + + # where joint is + readback = Cpt(EpicsSignalRO, "RB") + # where joint should go + setpoint = Cpt(EpicsSignal, "SP") + + +class Robotic_Arm(Device): + + # class init + def __init__(self, prefix="", *, name, **kwargs): + super().__init__(prefix=prefix, name=name, **kwargs) + + if not rclpy.ok(): + rclpy.init() + + self.ros = ROS_Node() + + # Joint components + base = Cpt(Joint, "base") + shoulder = Cpt(Joint, "shoulder") + elbow = Cpt(Joint, "elbow") + w1 = Cpt(Joint, "w1") + w2 = Cpt(Joint, "w2") + w3 = Cpt(Joint, "w3") + + def list_topics(self): + time.sleep(2) + topics = self.ros.list_topics() + for topic, types in topics: + print(f"{topic}: {types}") + +if __name__ == "__main__": + arm = Robotic_Arm(name="arm") + arm.list_topics() \ No newline at end of file From a0e0595247f90beac899eb01301dc73da1ed5793 Mon Sep 17 00:00:00 2001 From: Jennipher Day Date: Tue, 21 Jul 2026 16:31:19 -0400 Subject: [PATCH 2/7] joint state added, early draft --- src/bluesky_ros/ophyd_draft.py | 124 ++++++++++++++++++++++++++------- 1 file changed, 99 insertions(+), 25 deletions(-) diff --git a/src/bluesky_ros/ophyd_draft.py b/src/bluesky_ros/ophyd_draft.py index e64eb617..fe113b3d 100644 --- a/src/bluesky_ros/ophyd_draft.py +++ b/src/bluesky_ros/ophyd_draft.py @@ -1,16 +1,22 @@ from ophyd import Device, Component as Cpt, EpicsSignal, EpicsSignalRO import rclpy from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import JoinState import time -""" -My first draft of an ophyd device yay! -""" - class ROS_Node(Node): def __init__(self): super().__init__("arm_node") + self._joint_state_subscription = self.create_subscription( + JointState, + "/joint_states", + joint_state_callback, + qos_profile_sensor_data, + ) + + def list_topics(self): return self.get_topic_names_and_types() @@ -18,25 +24,13 @@ class Joint(Device): def __init__(self, device, **kwargs): super().__init__(device, **kwargs) - # ros topic - can you see it, use rclpy to list it - - # where joint is - readback = Cpt(EpicsSignalRO, "RB") - # where joint should go - setpoint = Cpt(EpicsSignal, "SP") - + # read from /joint_states topic + readback = Cpt(Signal, value=None) + velocity = Cpt(Signal, value=None) + effort = Cpt(Signal, value=None) class Robotic_Arm(Device): - # class init - def __init__(self, prefix="", *, name, **kwargs): - super().__init__(prefix=prefix, name=name, **kwargs) - - if not rclpy.ok(): - rclpy.init() - - self.ros = ROS_Node() - # Joint components base = Cpt(Joint, "base") shoulder = Cpt(Joint, "shoulder") @@ -45,12 +39,92 @@ def __init__(self, prefix="", *, name, **kwargs): w2 = Cpt(Joint, "w2") w3 = Cpt(Joint, "w3") + def __init__(self, prefix="", *, name, **kwargs): + super().__init__(prefix=prefix, name=name, **kwargs) + + if not rclpy.ok(): + rclpy.init() + + # Change the keys if /joint_states uses different joint names. + self._joint_name_map = { + "base": self.base, + "shoulder": self.shoulder, + "elbow": self.elbow, + "w1": self.w1, + "w2": self.w2, + "w3": self.w3, + } + + # ROS node calls self._joint_state_callback when data arrives. + self._ros_node = ROS_Node(self._joint_state_callback) + + def _joint_state_callback(self, message): + """ + Transfer values from a ROS JointState message into Ophyd Signals. + + The arrays in JointState are parallel: + message.name[i] + message.position[i] + message.velocity[i] + message.effort[i] + all describe the same joint. + """ + for index, ros_joint_name in enumerate(message.name): + joint = self._joint_name_map.get(ros_joint_name) + + if joint is None: + self._ros_node.get_logger().warning( + f"Unrecognized joint: {ros_joint_name}" + ) + continue + + if index < len(message.position): + joint.readback.put(message.position[index]) + + if index < len(message.velocity): + joint.velocity.put(message.velocity[index]) + + if index < len(message.effort): + joint.effort.put(message.effort[index]) + def list_topics(self): - time.sleep(2) - topics = self.ros.list_topics() - for topic, types in topics: - print(f"{topic}: {types}") + topics = self._ros_node.list_topics() + + for topic_name, topic_types in topics: + print(f"{topic_name}: {topic_types}") + + return topics + + def print_joint_states(self): + for joint_name, joint in self._joint_name_map.items(): + print( + f"{joint_name:10} " + f"position={joint.readback.get()} " + f"velocity={joint.velocity.get()} " + f"effort={joint.effort.get()}" + ) + + def close(self): + self._ros_node.destroy_node() + + if rclpy.ok(): + rclpy.shutdown() + + if __name__ == "__main__": arm = Robotic_Arm(name="arm") - arm.list_topics() \ No newline at end of file + + try: + print("Listening to /joint_states. Press Ctrl+C to stop.") + + while rclpy.ok(): + arm.process_ros_events(timeout_sec=0.1) + arm.print_joint_states() + print("-" * 70) + + except KeyboardInterrupt: + print("\nStopping.") + + finally: + arm.close() \ No newline at end of file From 96bab298a556eef1e8b65773f8d749cdb1fac605 Mon Sep 17 00:00:00 2001 From: Jennipher Day Date: Wed, 22 Jul 2026 14:01:06 -0400 Subject: [PATCH 3/7] joint states finished --- src/bluesky_ros/ophyd_draft.py | 55 +++++++++++++++++++++++++--------- 1 file changed, 41 insertions(+), 14 deletions(-) diff --git a/src/bluesky_ros/ophyd_draft.py b/src/bluesky_ros/ophyd_draft.py index fe113b3d..108a036c 100644 --- a/src/bluesky_ros/ophyd_draft.py +++ b/src/bluesky_ros/ophyd_draft.py @@ -1,12 +1,15 @@ -from ophyd import Device, Component as Cpt, EpicsSignal, EpicsSignalRO +from ophyd import Device, Signal +from ophyd import Component as Cpt + import rclpy from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data -from sensor_msgs.msg import JoinState +from sensor_msgs.msg import JointState import time class ROS_Node(Node): - def __init__(self): + + def __init__(self, joint_state_callback): super().__init__("arm_node") self._joint_state_subscription = self.create_subscription( @@ -16,13 +19,13 @@ def __init__(self): qos_profile_sensor_data, ) + self.get_logger().info("Subscribed to /joint_states") + def list_topics(self): return self.get_topic_names_and_types() class Joint(Device): - def __init__(self, device, **kwargs): - super().__init__(device, **kwargs) # read from /joint_states topic readback = Cpt(Signal, value=None) @@ -69,6 +72,9 @@ def _joint_state_callback(self, message): message.effort[i] all describe the same joint. """ + + updated_joints = [] + for index, ros_joint_name in enumerate(message.name): joint = self._joint_name_map.get(ros_joint_name) @@ -78,6 +84,8 @@ def _joint_state_callback(self, message): ) continue + # edge case: position, velocity, and effort arrays may be empty + if index < len(message.position): joint.readback.put(message.position[index]) @@ -87,6 +95,19 @@ def _joint_state_callback(self, message): if index < len(message.effort): joint.effort.put(message.effort[index]) + updated_joints.append(ros_joint_name) + + if updated_joints: + self.print_joint_states(updated_joints) + + + def process_ros_events(self, timeout_sec: float = 0.1) -> None: + if rclpy.ok(): + rclpy.spin_once( + self._ros_node, + timeout_sec = timeout_sec + ) + def list_topics(self): topics = self._ros_node.list_topics() @@ -95,13 +116,20 @@ def list_topics(self): return topics - def print_joint_states(self): - for joint_name, joint in self._joint_name_map.items(): + def print_joint_states(self, joint_names=None) -> None: + if joint_names is None: + joint_names = self._joint_name_map.keys() + + print("-" * 78) + + for joint_name in joint_names: + joint = self._joint_name_map[joint_name] + print( - f"{joint_name:10} " - f"position={joint.readback.get()} " - f"velocity={joint.velocity.get()} " - f"effort={joint.effort.get()}" + f"{joint_name:<10} " + f"position={joint.readback.get():>12.6f} " + f"velocity={joint.velocity.get():>12.6f} " + f"effort={joint.effort.get():>12.6f}" ) def close(self): @@ -117,12 +145,11 @@ def close(self): try: print("Listening to /joint_states. Press Ctrl+C to stop.") + print("Expected joint names:", ", ".join(arm._joint_name_map)) while rclpy.ok(): arm.process_ros_events(timeout_sec=0.1) - arm.print_joint_states() - print("-" * 70) - + except KeyboardInterrupt: print("\nStopping.") From 6f398fed13b7d244cd1812d1d57574f318000a06 Mon Sep 17 00:00:00 2001 From: Jennipher Day Date: Thu, 23 Jul 2026 13:08:51 -0400 Subject: [PATCH 4/7] Included some documentation --- src/bluesky_ros/ophyd_draft.py | 67 ++++++++++++++++++++++------------ 1 file changed, 43 insertions(+), 24 deletions(-) diff --git a/src/bluesky_ros/ophyd_draft.py b/src/bluesky_ros/ophyd_draft.py index 108a036c..b036bfcc 100644 --- a/src/bluesky_ros/ophyd_draft.py +++ b/src/bluesky_ros/ophyd_draft.py @@ -5,8 +5,12 @@ from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data from sensor_msgs.msg import JointState -import time +''' +ROS_Node +Creates and subscribes a ROS Node to /joint_states +to obtain joint data +''' class ROS_Node(Node): def __init__(self, joint_state_callback): @@ -22,9 +26,16 @@ def __init__(self, joint_state_callback): self.get_logger().info("Subscribed to /joint_states") - def list_topics(self): - return self.get_topic_names_and_types() + # def list_topics(self): + # return self.get_topic_names_and_types() +''' +Joint Class +Each joint stores three values: + - Readback: joint position (rad) + - Velocity: speed at which the joint is moving (rad/s) + - Effort: load/ mechanical output at arm joint (N*m) +''' class Joint(Device): # read from /joint_states topic @@ -32,15 +43,20 @@ class Joint(Device): velocity = Cpt(Signal, value=None) effort = Cpt(Signal, value=None) +''' +Robotic_Arm Class +Converts ROS messages into Ophyd signals. +Has 6 components that are Joint Devices. +''' class Robotic_Arm(Device): # Joint components - base = Cpt(Joint, "base") - shoulder = Cpt(Joint, "shoulder") - elbow = Cpt(Joint, "elbow") - w1 = Cpt(Joint, "w1") - w2 = Cpt(Joint, "w2") - w3 = Cpt(Joint, "w3") + Base = Cpt(Joint, "Base") + Shoulder = Cpt(Joint, "Shoulder") + Elbow = Cpt(Joint, "Elbow") + Wrist_1 = Cpt(Joint, "Wrist_1") + Wrist_2 = Cpt(Joint, "Wrist_2") + Wrist_3 = Cpt(Joint, "Wrist_3") def __init__(self, prefix="", *, name, **kwargs): super().__init__(prefix=prefix, name=name, **kwargs) @@ -48,17 +64,16 @@ def __init__(self, prefix="", *, name, **kwargs): if not rclpy.ok(): rclpy.init() - # Change the keys if /joint_states uses different joint names. + # Change the keys if /joint_states uses different joint names self._joint_name_map = { - "base": self.base, - "shoulder": self.shoulder, - "elbow": self.elbow, - "w1": self.w1, - "w2": self.w2, - "w3": self.w3, + "Base": self.Base, + "Shoulder": self.Shoulder, + "Elbow": self.Elbow, + "Wrist_1": self.Wrist_1, + "Wrist_2": self.Wrist_2, + "Wrist_3": self.Wrist_3, } - # ROS node calls self._joint_state_callback when data arrives. self._ros_node = ROS_Node(self._joint_state_callback) def _joint_state_callback(self, message): @@ -84,8 +99,6 @@ def _joint_state_callback(self, message): ) continue - # edge case: position, velocity, and effort arrays may be empty - if index < len(message.position): joint.readback.put(message.position[index]) @@ -102,21 +115,27 @@ def _joint_state_callback(self, message): def process_ros_events(self, timeout_sec: float = 0.1) -> None: + ''' + Waits and processes ROS events. + ''' if rclpy.ok(): rclpy.spin_once( self._ros_node, timeout_sec = timeout_sec ) - def list_topics(self): - topics = self._ros_node.list_topics() + # def list_topics(self): + # topics = self._ros_node.list_topics() - for topic_name, topic_types in topics: - print(f"{topic_name}: {topic_types}") + # for topic_name, topic_types in topics: + # print(f"{topic_name}: {topic_types}") - return topics + # return topics def print_joint_states(self, joint_names=None) -> None: + ''' + Prints current values of each Ophyd joint. + ''' if joint_names is None: joint_names = self._joint_name_map.keys() From e7f655fd2927607843da7885595d86827dfd7f36 Mon Sep 17 00:00:00 2001 From: Jennipher Day Date: Fri, 24 Jul 2026 14:50:48 -0400 Subject: [PATCH 5/7] updated joint names --- src/bluesky_ros/ophyd_draft.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/bluesky_ros/ophyd_draft.py b/src/bluesky_ros/ophyd_draft.py index b036bfcc..54c44c6c 100644 --- a/src/bluesky_ros/ophyd_draft.py +++ b/src/bluesky_ros/ophyd_draft.py @@ -51,12 +51,12 @@ class Joint(Device): class Robotic_Arm(Device): # Joint components - Base = Cpt(Joint, "Base") - Shoulder = Cpt(Joint, "Shoulder") - Elbow = Cpt(Joint, "Elbow") - Wrist_1 = Cpt(Joint, "Wrist_1") - Wrist_2 = Cpt(Joint, "Wrist_2") - Wrist_3 = Cpt(Joint, "Wrist_3") + shoulder_pan_joint = Cpt(Joint, "shoulder_pan_joint") + shoulder_lift_joint = Cpt(Joint, "shoulder_lift_joint") + elbow_join = Cpt(Joint, "elbow_join") + wrist_1_joint = Cpt(Joint, "wrist_1_joint") + wrist_2_joint = Cpt(Joint, "wrist_2_joint") + wrist_3_joint = Cpt(Joint, "wrist_3_joint") def __init__(self, prefix="", *, name, **kwargs): super().__init__(prefix=prefix, name=name, **kwargs) @@ -66,12 +66,12 @@ def __init__(self, prefix="", *, name, **kwargs): # Change the keys if /joint_states uses different joint names self._joint_name_map = { - "Base": self.Base, - "Shoulder": self.Shoulder, - "Elbow": self.Elbow, - "Wrist_1": self.Wrist_1, - "Wrist_2": self.Wrist_2, - "Wrist_3": self.Wrist_3, + "shoulder_pan_joint": self.shoulder_pan_joint, # shoulder_pan + "shoulder_lift_joint": self.shoulder_lift_joint, # shoulder_lift + "elbow_join": self.elbow_join, + "wrist_1_joint": self.wrist_1_joint, + "wrist_2_joint": self.wrist_2_joint, + "wrist_3_joint": self.wrist_3_joint, } self._ros_node = ROS_Node(self._joint_state_callback) From 7091df6933bdfecf8efe7db0fa26b5768ab65a51 Mon Sep 17 00:00:00 2001 From: Jennipher Day Date: Fri, 24 Jul 2026 14:55:15 -0400 Subject: [PATCH 6/7] updated joint names (again lol) --- src/bluesky_ros/ophyd_draft.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/bluesky_ros/ophyd_draft.py b/src/bluesky_ros/ophyd_draft.py index 54c44c6c..774a5310 100644 --- a/src/bluesky_ros/ophyd_draft.py +++ b/src/bluesky_ros/ophyd_draft.py @@ -53,7 +53,7 @@ class Robotic_Arm(Device): # Joint components shoulder_pan_joint = Cpt(Joint, "shoulder_pan_joint") shoulder_lift_joint = Cpt(Joint, "shoulder_lift_joint") - elbow_join = Cpt(Joint, "elbow_join") + elbow_joint = Cpt(Joint, "elbow_joint") wrist_1_joint = Cpt(Joint, "wrist_1_joint") wrist_2_joint = Cpt(Joint, "wrist_2_joint") wrist_3_joint = Cpt(Joint, "wrist_3_joint") @@ -68,7 +68,7 @@ def __init__(self, prefix="", *, name, **kwargs): self._joint_name_map = { "shoulder_pan_joint": self.shoulder_pan_joint, # shoulder_pan "shoulder_lift_joint": self.shoulder_lift_joint, # shoulder_lift - "elbow_join": self.elbow_join, + "elbow_joint": self.elbow_joint, "wrist_1_joint": self.wrist_1_joint, "wrist_2_joint": self.wrist_2_joint, "wrist_3_joint": self.wrist_3_joint, From 44a57485b4bb82277660ce87ff3e7bfe203a6922 Mon Sep 17 00:00:00 2001 From: Jen Date: Fri, 7 Aug 2026 11:42:33 -0400 Subject: [PATCH 7/7] Renamed ophyd_joints.py file --- src/bluesky_ros/{ophyd_draft.py => ophyd_joints.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/bluesky_ros/{ophyd_draft.py => ophyd_joints.py} (99%) diff --git a/src/bluesky_ros/ophyd_draft.py b/src/bluesky_ros/ophyd_joints.py similarity index 99% rename from src/bluesky_ros/ophyd_draft.py rename to src/bluesky_ros/ophyd_joints.py index 774a5310..ff2088b6 100644 --- a/src/bluesky_ros/ophyd_draft.py +++ b/src/bluesky_ros/ophyd_joints.py @@ -173,4 +173,4 @@ def close(self): print("\nStopping.") finally: - arm.close() \ No newline at end of file + arm.close()