别人克隆后没有 install，source 会直接失败。把展开后的 install 放进仓库，符号链接和指向本机 build 的 egg-link 收成真实文件，Python 入口改走当前环境的解释器，这样不用先 colcon build 也能启动。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+33
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'flexiv-test-nodes','console_scripts','publisher_joint_trajectory_controller'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'flexiv-test-nodes'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('flexiv-test-nodes', 'console_scripts', 'publisher_joint_trajectory_controller')())
|
||||
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'flexiv-test-nodes','console_scripts','robot_states_monitor'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'flexiv-test-nodes'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('flexiv-test-nodes', 'console_scripts', 'robot_states_monitor')())
|
||||
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'flexiv-test-nodes','console_scripts','robot_states_publisher'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'flexiv-test-nodes'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('flexiv-test-nodes', 'console_scripts', 'robot_states_publisher')())
|
||||
+27
@@ -0,0 +1,27 @@
|
||||
Metadata-Version: 2.4
|
||||
Name: flexiv_test_nodes
|
||||
Version: 0.0.1
|
||||
Summary: Demo nodes for testing flexiv_ros2.
|
||||
Author: Mun Seng Phoon
|
||||
Author-email: munseng.phoon@flexiv.com
|
||||
Maintainer: Mun Seng Phoon
|
||||
Maintainer-email: munseng.phoon@flexiv.com
|
||||
License: Apache License, Version 2.0
|
||||
Keywords: ROS
|
||||
Classifier: Intended Audience :: Developers
|
||||
Classifier: License :: OSI Approved :: Apache Software License
|
||||
Classifier: Programming Language :: Python
|
||||
Classifier: Topic :: Software Development
|
||||
Requires-Dist: setuptools
|
||||
Dynamic: author
|
||||
Dynamic: author-email
|
||||
Dynamic: classifier
|
||||
Dynamic: description
|
||||
Dynamic: keywords
|
||||
Dynamic: license
|
||||
Dynamic: maintainer
|
||||
Dynamic: maintainer-email
|
||||
Dynamic: requires-dist
|
||||
Dynamic: summary
|
||||
|
||||
Demo nodes for for testing flexiv_ros2.
|
||||
+16
@@ -0,0 +1,16 @@
|
||||
package.xml
|
||||
setup.cfg
|
||||
setup.py
|
||||
flexiv_test_nodes/__init__.py
|
||||
flexiv_test_nodes/publisher_joint_trajectory_controller.py
|
||||
flexiv_test_nodes/robot_states_monitor.py
|
||||
flexiv_test_nodes/robot_states_publisher.py
|
||||
flexiv_test_nodes.egg-info/PKG-INFO
|
||||
flexiv_test_nodes.egg-info/SOURCES.txt
|
||||
flexiv_test_nodes.egg-info/dependency_links.txt
|
||||
flexiv_test_nodes.egg-info/entry_points.txt
|
||||
flexiv_test_nodes.egg-info/requires.txt
|
||||
flexiv_test_nodes.egg-info/top_level.txt
|
||||
flexiv_test_nodes.egg-info/zip-safe
|
||||
launch/robot_states_publisher.launch.py
|
||||
resource/flexiv_test_nodes
|
||||
+1
@@ -0,0 +1 @@
|
||||
|
||||
+4
@@ -0,0 +1,4 @@
|
||||
[console_scripts]
|
||||
publisher_joint_trajectory_controller = flexiv_test_nodes.publisher_joint_trajectory_controller:main
|
||||
robot_states_monitor = flexiv_test_nodes.robot_states_monitor:main
|
||||
robot_states_publisher = flexiv_test_nodes.robot_states_publisher:main
|
||||
+1
@@ -0,0 +1 @@
|
||||
setuptools
|
||||
+1
@@ -0,0 +1 @@
|
||||
flexiv_test_nodes
|
||||
+1
@@ -0,0 +1 @@
|
||||
|
||||
+148
@@ -0,0 +1,148 @@
|
||||
# Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt)
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import rclpy
|
||||
from builtin_interfaces.msg import Duration
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from rcl_interfaces.msg import ParameterDescriptor
|
||||
|
||||
|
||||
class PublisherJointTrajectory(Node):
|
||||
def __init__(self):
|
||||
super().__init__("publisher_position_trajectory_controller")
|
||||
# Declare all parameters
|
||||
self.declare_parameter("controller_name", "joint_trajectory_controller")
|
||||
self.declare_parameter("wait_sec_between_publish", 6)
|
||||
self.declare_parameter("goal_names", ["pos1", "pos2"])
|
||||
self.declare_parameter("joints", [""])
|
||||
self.declare_parameter("check_starting_point", False)
|
||||
self.declare_parameter("starting_point_limits", False)
|
||||
|
||||
# Read parameters
|
||||
controller_name = self.get_parameter("controller_name").value
|
||||
wait_sec_between_publish = self.get_parameter("wait_sec_between_publish").value
|
||||
goal_names = self.get_parameter("goal_names").value
|
||||
self.joints = self.get_parameter("joints").value
|
||||
self.check_starting_point = self.get_parameter("check_starting_point").value
|
||||
self.starting_point = {}
|
||||
|
||||
if self.joints is None or len(self.joints) == 0:
|
||||
raise Exception('"joints" parameter is not set!')
|
||||
|
||||
# starting point stuff
|
||||
if self.check_starting_point:
|
||||
# declare nested params
|
||||
for name in self.joints:
|
||||
param_name_tmp = "starting_point_limits" + "." + name
|
||||
self.declare_parameter(param_name_tmp, [-2 * 3.14159, 2 * 3.14159])
|
||||
self.starting_point[name] = self.get_parameter(param_name_tmp).value
|
||||
|
||||
for name in self.joints:
|
||||
if len(self.starting_point[name]) != 2:
|
||||
raise Exception('"starting_point" parameter is not set correctly!')
|
||||
self.joint_state_sub = self.create_subscription(
|
||||
JointState, "joint_states", self.joint_state_callback, 10
|
||||
)
|
||||
# initialize starting point status
|
||||
self.starting_point_ok = not self.check_starting_point
|
||||
|
||||
self.joint_state_msg_received = False
|
||||
|
||||
# Read all positions from parameters
|
||||
self.goals = []
|
||||
for name in goal_names:
|
||||
self.declare_parameter(
|
||||
name, descriptor=ParameterDescriptor(dynamic_typing=True)
|
||||
)
|
||||
goal = self.get_parameter(name).value
|
||||
if goal is None or len(goal) == 0:
|
||||
raise Exception(f'Values for goal "{name}" not set!')
|
||||
|
||||
float_goal = []
|
||||
for value in goal:
|
||||
float_goal.append(float(value))
|
||||
self.goals.append(float_goal)
|
||||
|
||||
publish_topic = "/" + controller_name + "/" + "joint_trajectory"
|
||||
|
||||
self.get_logger().info(
|
||||
'Publishing {} goals on topic "{}" every {} s'.format(
|
||||
len(goal_names), publish_topic, wait_sec_between_publish
|
||||
)
|
||||
)
|
||||
|
||||
self.publisher_ = self.create_publisher(JointTrajectory, publish_topic, 1)
|
||||
self.timer = self.create_timer(wait_sec_between_publish, self.timer_callback)
|
||||
self.i = 0
|
||||
|
||||
def timer_callback(self):
|
||||
if self.starting_point_ok:
|
||||
traj = JointTrajectory()
|
||||
traj.joint_names = self.joints
|
||||
point = JointTrajectoryPoint()
|
||||
point.positions = self.goals[self.i]
|
||||
point.time_from_start = Duration(sec=1)
|
||||
|
||||
traj.points.append(point)
|
||||
self.publisher_.publish(traj)
|
||||
|
||||
self.i += 1
|
||||
self.i %= len(self.goals)
|
||||
|
||||
elif self.check_starting_point and not self.joint_state_msg_received:
|
||||
self.get_logger().warn(
|
||||
'Start configuration could not be checked! Check "joint_state" topic!'
|
||||
)
|
||||
else:
|
||||
self.get_logger().warn(
|
||||
"Start configuration is not within configured limits!"
|
||||
)
|
||||
|
||||
def joint_state_callback(self, msg):
|
||||
if not self.joint_state_msg_received:
|
||||
# check start state
|
||||
limit_exceeded = [False] * len(msg.name)
|
||||
for idx, enum in enumerate(msg.name):
|
||||
if (msg.position[idx] < self.starting_point[enum][0]) or (
|
||||
msg.position[idx] > self.starting_point[enum][1]
|
||||
):
|
||||
self.get_logger().warn(
|
||||
f"Starting point limits exceeded for joint {enum} !"
|
||||
)
|
||||
limit_exceeded[idx] = True
|
||||
|
||||
if any(limit_exceeded):
|
||||
self.starting_point_ok = False
|
||||
else:
|
||||
self.starting_point_ok = True
|
||||
|
||||
self.joint_state_msg_received = True
|
||||
else:
|
||||
return
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
publisher_joint_trajectory = PublisherJointTrajectory()
|
||||
|
||||
rclpy.spin(publisher_joint_trajectory)
|
||||
publisher_joint_trajectory.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+85
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Example subscriber node demonstrating how to receive and process robot
|
||||
states published by the robot_states_publisher node.
|
||||
|
||||
Author: Flexiv Robotics
|
||||
License: Apache-2.0
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from flexiv_msgs.msg import RobotStates
|
||||
import math
|
||||
|
||||
|
||||
class StateMonitor(Node):
|
||||
"""Example ROS2 node that subscribes to robot states and displays key
|
||||
information."""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("state_monitor")
|
||||
|
||||
# Declare a parameter for the robot serial number
|
||||
self.declare_parameter("robot_sn", "Rizon4-000000")
|
||||
robot_sn = self.get_parameter("robot_sn").get_parameter_value().string_value
|
||||
|
||||
# Construct topic name dynamically based on robot SN
|
||||
# ROS topic names can't contain dashes, so replace with underscores
|
||||
topic_robot_sn = robot_sn.replace("-", "_")
|
||||
topic_name = f"/{topic_robot_sn}/flexiv_robot_states"
|
||||
|
||||
self.get_logger().info(f"State Monitor started")
|
||||
self.get_logger().info(f"Subscribing to topic: {topic_name}")
|
||||
|
||||
# Create subscription
|
||||
self.subscription = self.create_subscription(
|
||||
RobotStates, topic_name, self.callback, 10
|
||||
)
|
||||
|
||||
def callback(self, msg):
|
||||
"""Callback function that processes received robot states.
|
||||
|
||||
Args:
|
||||
msg: RobotStates message containing complete robot state information
|
||||
"""
|
||||
# Access joint positions (7 joints)
|
||||
self.get_logger().info(f'Joint positions: {[f"{j:.3f}" for j in msg.q]}')
|
||||
|
||||
# Access TCP position
|
||||
tcp = msg.tcp_pose.pose.position
|
||||
self.get_logger().info(
|
||||
f"TCP Position: x={tcp.x:.3f}, y={tcp.y:.3f}, z={tcp.z:.3f}"
|
||||
)
|
||||
|
||||
# Access TCP orientation (quaternion)
|
||||
tcp_ori = msg.tcp_pose.pose.orientation
|
||||
self.get_logger().info(
|
||||
f"TCP Orientation: w={tcp_ori.w:.3f}, x={tcp_ori.x:.3f}, "
|
||||
f"y={tcp_ori.y:.3f}, z={tcp_ori.z:.3f}"
|
||||
)
|
||||
|
||||
# Access external force magnitude
|
||||
force = msg.ext_wrench_in_tcp.wrench.force
|
||||
force_mag = math.sqrt(force.x**2 + force.y**2 + force.z**2)
|
||||
self.get_logger().info(f"External Force Magnitude: {force_mag:.3f} N")
|
||||
|
||||
self.get_logger().info("---")
|
||||
|
||||
|
||||
def main(args=None):
|
||||
"""Main entry point for the State Monitor example node."""
|
||||
rclpy.init(args=args)
|
||||
|
||||
try:
|
||||
node = StateMonitor()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
print("\nKeyboard interrupt, shutting down...")
|
||||
finally:
|
||||
if "node" in locals():
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+304
@@ -0,0 +1,304 @@
|
||||
#!/usr/bin/env python3
|
||||
"""This node reads robot states from Flexiv RDK and publishes them as ROS2
|
||||
messages (flexiv_msgs/msg/RobotStates).
|
||||
|
||||
Author: Flexiv Robotics
|
||||
License: Apache-2.0
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
import flexivrdk
|
||||
import sys
|
||||
import argparse
|
||||
|
||||
# Import ROS2 message types
|
||||
from flexiv_msgs.msg import RobotStates
|
||||
from geometry_msgs.msg import PoseStamped, AccelStamped, WrenchStamped
|
||||
|
||||
|
||||
class RobotStatesPublisher(Node):
|
||||
"""ROS2 node that publishes Flexiv robot states from RDK to ROS2 topics."""
|
||||
|
||||
def __init__(self, robot_sn: str, network_interface: str = ""):
|
||||
super().__init__("rdk_state_publisher")
|
||||
|
||||
# Declare parameters
|
||||
self.declare_parameter("robot_sn", robot_sn)
|
||||
self.declare_parameter("network_interface", network_interface)
|
||||
self.declare_parameter("publish_rate", 100) # Default 100 Hz
|
||||
|
||||
# Get parameters
|
||||
self.robot_sn = self.get_parameter("robot_sn").value
|
||||
self.network_interface = self.get_parameter("network_interface").value
|
||||
self.publish_rate = self.get_parameter("publish_rate").value
|
||||
|
||||
self.get_logger().info(
|
||||
f"Initializing Robot States Publisher for robot {self.robot_sn}"
|
||||
)
|
||||
if self.network_interface:
|
||||
self.get_logger().info(
|
||||
f"Network interface whitelist: [{self.network_interface}]"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info("Using all available network interfaces")
|
||||
self.get_logger().info(f"Publish rate: {self.publish_rate} Hz")
|
||||
|
||||
# Initialize RDK Robot instance
|
||||
try:
|
||||
if self.network_interface:
|
||||
# Use specific network interface
|
||||
self.robot = flexivrdk.Robot(self.robot_sn, [self.network_interface])
|
||||
else:
|
||||
# Use all available network interfaces
|
||||
self.robot = flexivrdk.Robot(self.robot_sn)
|
||||
self.get_logger().info("Successfully connected to robot via RDK")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to connect to robot: {str(e)}")
|
||||
raise
|
||||
|
||||
# Setup QoS profile for real-time communication
|
||||
qos_profile = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
|
||||
# Create publisher for robot states
|
||||
# Sanitize robot_sn for ROS2 topic name (replace dashes with underscores)
|
||||
topic_name = f'/{self.robot_sn.replace("-", "_")}/flexiv_robot_states'
|
||||
self.state_publisher = self.create_publisher(
|
||||
RobotStates, topic_name, qos_profile
|
||||
)
|
||||
|
||||
# Create timer for periodic publishing
|
||||
timer_period = 1.0 / self.publish_rate
|
||||
self.timer = self.create_timer(timer_period, self.publish_robot_states)
|
||||
|
||||
self.get_logger().info("Robot States Publisher initialized successfully")
|
||||
self.get_logger().info(f"Publishing to: {topic_name}")
|
||||
|
||||
# Log robot operational status
|
||||
self.log_robot_status()
|
||||
|
||||
def log_robot_status(self):
|
||||
"""Log the current robot status from RDK."""
|
||||
try:
|
||||
is_busy = self.robot.busy()
|
||||
is_operational = self.robot.operational()
|
||||
is_fault = self.robot.fault()
|
||||
is_reduced = self.robot.reduced()
|
||||
|
||||
self.get_logger().info("=== Robot Status ===")
|
||||
self.get_logger().info(f"Busy: {is_busy}")
|
||||
self.get_logger().info(f"Operational: {is_operational}")
|
||||
self.get_logger().info(f"Fault: {is_fault}")
|
||||
self.get_logger().info(f"Reduced Mode: {is_reduced}")
|
||||
self.get_logger().info("===================")
|
||||
|
||||
if is_fault:
|
||||
self.get_logger().warn("Robot is in FAULT state!")
|
||||
elif not is_operational:
|
||||
self.get_logger().warn("Robot is NOT operational!")
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Error checking robot status: {str(e)}")
|
||||
|
||||
def create_pose_stamped(self, pose_data, frame_id="world"):
|
||||
"""Convert RDK pose data [x, y, z, q_w, q_x, q_y, q_z] to
|
||||
PoseStamped."""
|
||||
pose_stamped = PoseStamped()
|
||||
pose_stamped.header.stamp = self.get_clock().now().to_msg()
|
||||
pose_stamped.header.frame_id = frame_id
|
||||
|
||||
pose_stamped.pose.position.x = pose_data[0]
|
||||
pose_stamped.pose.position.y = pose_data[1]
|
||||
pose_stamped.pose.position.z = pose_data[2]
|
||||
|
||||
pose_stamped.pose.orientation.w = pose_data[3]
|
||||
pose_stamped.pose.orientation.x = pose_data[4]
|
||||
pose_stamped.pose.orientation.y = pose_data[5]
|
||||
pose_stamped.pose.orientation.z = pose_data[6]
|
||||
|
||||
return pose_stamped
|
||||
|
||||
def create_accel_stamped(self, vel_data, frame_id="world"):
|
||||
"""
|
||||
Convert RDK velocity data [v_x, v_y, v_z, w_x, w_y, w_z] to AccelStamped
|
||||
Note: Using AccelStamped to hold velocity data as per RobotStates.msg
|
||||
"""
|
||||
accel_stamped = AccelStamped()
|
||||
accel_stamped.header.stamp = self.get_clock().now().to_msg()
|
||||
accel_stamped.header.frame_id = frame_id
|
||||
|
||||
# Linear velocity
|
||||
accel_stamped.accel.linear.x = vel_data[0]
|
||||
accel_stamped.accel.linear.y = vel_data[1]
|
||||
accel_stamped.accel.linear.z = vel_data[2]
|
||||
|
||||
# Angular velocity
|
||||
accel_stamped.accel.angular.x = vel_data[3]
|
||||
accel_stamped.accel.angular.y = vel_data[4]
|
||||
accel_stamped.accel.angular.z = vel_data[5]
|
||||
|
||||
return accel_stamped
|
||||
|
||||
def create_wrench_stamped(self, wrench_data, frame_id):
|
||||
"""Convert RDK wrench data [f_x, f_y, f_z, m_x, m_y, m_z] to
|
||||
WrenchStamped."""
|
||||
wrench_stamped = WrenchStamped()
|
||||
wrench_stamped.header.stamp = self.get_clock().now().to_msg()
|
||||
wrench_stamped.header.frame_id = frame_id
|
||||
|
||||
# Force
|
||||
wrench_stamped.wrench.force.x = wrench_data[0]
|
||||
wrench_stamped.wrench.force.y = wrench_data[1]
|
||||
wrench_stamped.wrench.force.z = wrench_data[2]
|
||||
|
||||
# Torque/Moment
|
||||
wrench_stamped.wrench.torque.x = wrench_data[3]
|
||||
wrench_stamped.wrench.torque.y = wrench_data[4]
|
||||
wrench_stamped.wrench.torque.z = wrench_data[5]
|
||||
|
||||
return wrench_stamped
|
||||
|
||||
def publish_robot_states(self):
|
||||
"""Main callback function to read robot states and publish as ROS2
|
||||
message."""
|
||||
try:
|
||||
# Get robot states from RDK
|
||||
robot_states = self.robot.states()
|
||||
|
||||
# Create ROS2 RobotStates message
|
||||
msg = RobotStates()
|
||||
|
||||
# Populate header
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = "world"
|
||||
|
||||
# Robot timestamp
|
||||
msg.robot_timestamp.sec = robot_states.timestamp[0]
|
||||
msg.robot_timestamp.nanosec = robot_states.timestamp[1]
|
||||
|
||||
# Joint-space states (all arrays are size 7 for Flexiv robots)
|
||||
msg.q = list(robot_states.q) # Joint positions (link-side)
|
||||
msg.theta = list(robot_states.theta) # Joint positions (motor-side)
|
||||
msg.dq = list(robot_states.dq) # Joint velocities (link-side)
|
||||
msg.dtheta = list(robot_states.dtheta) # Joint velocities (motor-side)
|
||||
msg.tau = list(robot_states.tau) # Joint torques
|
||||
msg.tau_des = list(robot_states.tau_des) # Desired joint torques
|
||||
msg.tau_dot = list(robot_states.tau_dot) # Joint torque derivatives
|
||||
msg.tau_ext = list(robot_states.tau_ext) # External joint torques
|
||||
msg.tau_interact = list(
|
||||
robot_states.tau_interact
|
||||
) # Interaction joint torques
|
||||
msg.temperature = list(robot_states.temperature) # Joint temperatures
|
||||
|
||||
# Cartesian-space states using geometry_msgs
|
||||
# TCP pose: [x, y, z, q_w, q_x, q_y, q_z]
|
||||
msg.tcp_pose = self.create_pose_stamped(robot_states.tcp_pose, "world")
|
||||
|
||||
# TCP velocity: [v_x, v_y, v_z, w_x, w_y, w_z]
|
||||
msg.tcp_vel = self.create_accel_stamped(robot_states.tcp_vel, "world")
|
||||
|
||||
# Flange pose: [x, y, z, q_w, q_x, q_y, q_z]
|
||||
msg.flange_pose = self.create_pose_stamped(
|
||||
robot_states.flange_pose, "world"
|
||||
)
|
||||
|
||||
# Force-torque sensor reading: [f_x, f_y, f_z, m_x, m_y, m_z]
|
||||
msg.ft_sensor_raw = self.create_wrench_stamped(
|
||||
robot_states.ft_sensor_raw, "flange"
|
||||
)
|
||||
|
||||
# External wrench in TCP frame: [f_x, f_y, f_z, m_x, m_y, m_z]
|
||||
msg.ext_wrench_in_tcp = self.create_wrench_stamped(
|
||||
robot_states.ext_wrench_in_tcp, "flange"
|
||||
)
|
||||
|
||||
# External wrench in world frame: [f_x, f_y, f_z, m_x, m_y, m_z]
|
||||
msg.ext_wrench_in_world = self.create_wrench_stamped(
|
||||
robot_states.ext_wrench_in_world, "world"
|
||||
)
|
||||
|
||||
# External wrench in TCP frame (raw): [f_x, f_y, f_z, m_x, m_y, m_z]
|
||||
msg.ext_wrench_in_tcp_raw = self.create_wrench_stamped(
|
||||
robot_states.ext_wrench_in_tcp_raw, "flange"
|
||||
)
|
||||
|
||||
# External wrench in world frame (raw): [f_x, f_y, f_z, m_x, m_y, m_z]
|
||||
msg.ext_wrench_in_world_raw = self.create_wrench_stamped(
|
||||
robot_states.ext_wrench_in_world_raw, "world"
|
||||
)
|
||||
|
||||
# Publish the message
|
||||
self.state_publisher.publish(msg)
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(
|
||||
f"Error publishing robot states: {str(e)}", throttle_duration_sec=1.0
|
||||
)
|
||||
|
||||
def destroy_node(self):
|
||||
"""Clean shutdown of the node."""
|
||||
self.get_logger().info("Shutting down Robot States Publisher...")
|
||||
try:
|
||||
# Stop the robot safely if needed
|
||||
if hasattr(self, "robot"):
|
||||
self.get_logger().info("Disconnecting from robot...")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Error during shutdown: {str(e)}")
|
||||
finally:
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
"""Main entry point for the Robot States Publisher node."""
|
||||
# Parse command line arguments
|
||||
parser = argparse.ArgumentParser(description="Flexiv Robot States Publisher Node")
|
||||
parser.add_argument(
|
||||
"--robot-sn",
|
||||
type=str,
|
||||
required=True,
|
||||
help="Robot serial number (e.g., Rizon4s-123456)",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--network-interface",
|
||||
type=str,
|
||||
default="",
|
||||
help="Network interface name (e.g., eth0, enp0s31f6). Leave empty to use all interfaces.",
|
||||
)
|
||||
|
||||
# Parse known args (ROS2 args are handled separately)
|
||||
parsed_args, unknown = parser.parse_known_args()
|
||||
|
||||
# Initialize ROS2
|
||||
rclpy.init(args=args)
|
||||
|
||||
try:
|
||||
# Create and spin the node
|
||||
node = RobotStatesPublisher(
|
||||
robot_sn=parsed_args.robot_sn,
|
||||
network_interface=parsed_args.network_interface,
|
||||
)
|
||||
|
||||
# Spin the node
|
||||
rclpy.spin(node)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\nKeyboard interrupt, shutting down...")
|
||||
except Exception as e:
|
||||
print(f"Error: {str(e)}")
|
||||
return 1
|
||||
finally:
|
||||
# Cleanup
|
||||
if "node" in locals():
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
Reference in New Issue
Block a user