AS: cart and joint trajectories added
This commit is contained in:
Alexander Schaefer
@@ -1,6 +1,6 @@
|
||||
AMENT_PREFIX_PATH=/BA/workspace/src/install/joint_info:/BA/workspace/src/install/joint_control:/opt/ros/humble
|
||||
AMENT_PREFIX_PATH=/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
|
||||
COLCON=1
|
||||
COLCON_PREFIX_PATH=/BA/workspace/src/install
|
||||
COLCON_PREFIX_PATH=/BA/workspace/install
|
||||
HOME=/root
|
||||
HOSTNAME=3230bc57d699
|
||||
LANG=C.UTF-8
|
||||
@@ -10,7 +10,7 @@ LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd
|
||||
OLDPWD=/BA/workspace/src
|
||||
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
|
||||
PWD=/BA/workspace/build/joint_control
|
||||
PYTHONPATH=/BA/workspace/src/build/joint_info:/BA/workspace/src/install/joint_info/lib/python3.10/site-packages:/BA/workspace/src/build/joint_control:/BA/workspace/src/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
PYTHONPATH=/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/BA/workspace/src/build/joint_info:/BA/workspace/src/install/joint_info/lib/python3.10/site-packages:/BA/workspace/src/build/joint_control:/BA/workspace/src/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
ROS_DISTRO=humble
|
||||
ROS_LOCALHOST_ONLY=0
|
||||
ROS_PYTHON_VERSION=3
|
||||
|
||||
@@ -2,7 +2,10 @@ package.xml
|
||||
setup.cfg
|
||||
setup.py
|
||||
joint_control/__init__.py
|
||||
joint_control/cart_tcp_server.py
|
||||
joint_control/joint_angles_server.py
|
||||
joint_control/trajectory_server.py
|
||||
joint_control/trajectory_server_cart.py
|
||||
joint_control.egg-info/PKG-INFO
|
||||
joint_control.egg-info/SOURCES.txt
|
||||
joint_control.egg-info/dependency_links.txt
|
||||
|
||||
@@ -1,3 +1,6 @@
|
||||
[console_scripts]
|
||||
cart_coords = joint_control.cart_tcp_server:main
|
||||
joint_control = joint_control.joint_angles_server:main
|
||||
trajectory_server = joint_control.trajectory_server:main
|
||||
trajectory_server_cart = joint_control.trajectory_server_cart:main
|
||||
|
||||
|
||||
Binary file not shown.
@@ -1,102 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscbuildparse
|
||||
|
||||
class JointStateOSC(Node):
|
||||
def __init__(self):
|
||||
super().__init__('joint_states_osc')
|
||||
|
||||
# Create a ROS 2 subscriber to /joint_states topic
|
||||
self.subscription = self.create_subscription(
|
||||
JointState,
|
||||
'/joint_states',
|
||||
self.joint_states_callback,
|
||||
1 # Queue size
|
||||
)
|
||||
|
||||
# Open Sound Control (OSC) Client settings
|
||||
self.osc_ip = "127.0.0.1" # Replace with the target IP
|
||||
self.osc_port = 8000 # Replace with the target port
|
||||
|
||||
# Start the OSC system
|
||||
osc_startup()
|
||||
|
||||
# Make client channels to send packets
|
||||
osc_udp_client(self.osc_ip, self.osc_port, "osc_client")
|
||||
|
||||
def joint_states_callback(self, msg):
|
||||
"""Callback function to handle incoming joint states."""
|
||||
header = msg.header
|
||||
joint_names = msg.name
|
||||
joint_positions = msg.position
|
||||
joint_velocity = msg.velocity
|
||||
joint_effort = msg.effort
|
||||
|
||||
joint_names_str = "\n- ".join(joint_names)
|
||||
joint_positions_str = "\n- ".join(map(str, joint_positions))
|
||||
joint_velocity_str = "\n- ".join(map(str, joint_velocity))
|
||||
joint_effort_str = "\n- ".join(map(str, joint_effort))
|
||||
|
||||
info = f"""
|
||||
---
|
||||
header:
|
||||
stamp:
|
||||
sec: {header.stamp.sec}
|
||||
nanosec: {header.stamp.nanosec}
|
||||
name:
|
||||
- {joint_names_str}
|
||||
position:
|
||||
- {joint_positions_str}
|
||||
velocity:
|
||||
- {joint_velocity_str}
|
||||
effort:
|
||||
- {joint_effort_str}
|
||||
---"""
|
||||
|
||||
# Send the info message
|
||||
msg_info = oscbuildparse.OSCMessage("/joint_states", None, info)
|
||||
msg_name = oscbuildparse.OSCMessage("/joint_states/name", None, type(joint_names))
|
||||
msg_position = oscbuildparse.OSCMessage("/joint_states/position", None, type(joint_positions))
|
||||
msg_velocity = oscbuildparse.OSCMessage("/joint_states/velocity", None, type(joint_velocity))
|
||||
msg_effort = oscbuildparse.OSCMessage("/joint_states/effort", None, type(joint_effort))
|
||||
|
||||
bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_info, msg_name, msg_position, msg_velocity, msg_effort])
|
||||
|
||||
osc_send(bun, "osc_client")
|
||||
osc_process()
|
||||
#print(f"Publishing: {info}")
|
||||
|
||||
|
||||
'''
|
||||
# Send each joint state as an OSC message
|
||||
for i, name in enumerate(joint_names):
|
||||
#msg_sec = oscbuildparse.OSCMessage(f"/joint_states/header/sec", None, [header.stamp.sec])
|
||||
#msg_nanosec = oscbuildparse.OSCMessage(f"/joint_states/header/nanosec", None, [header.stamp.nanosec])
|
||||
msg_position = oscbuildparse.OSCMessage(f"/joint_states/{name}/position", None, [joint_positions[i]])
|
||||
msg_velocity = oscbuildparse.OSCMessage(f"/joint_states/{name}/velocity", None, [joint_velocity[i]])
|
||||
msg_effort = oscbuildparse.OSCMessage(f"/joint_states/{name}/effort", None, [joint_effort[i]])
|
||||
|
||||
bun = oscbuildparse.OSCBundle(oscbuildparse.unixtime2timetag(header.stamp.sec + header.stamp.nanosec), [msg_position, msg_velocity, msg_effort])
|
||||
#bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY , [msg_position, msg_velocity, msg_effort])
|
||||
osc_send(bun, "osc_client")
|
||||
osc_process()
|
||||
#print(f"OSC bundle sent for joint {name}")
|
||||
'''
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = JointStateOSC()
|
||||
print(f"Publishing joint states to OSC on {node.osc_ip}:{node.osc_port}...")
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
print("shutting down...")
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -1,39 +0,0 @@
|
||||
from osc4py3.as_eventloop import *
|
||||
from osc4py3 import oscmethod as osm
|
||||
import time
|
||||
|
||||
def joint_states_handler(*args):
|
||||
"""Handler function to process incoming joint states."""
|
||||
for arg in args:
|
||||
print(arg)
|
||||
|
||||
def main():
|
||||
ip = "0.0.0.0" # IP address to listen on
|
||||
port = 8000 # Port to listen on
|
||||
|
||||
# Start the OSC system
|
||||
osc_startup()
|
||||
|
||||
# Make server channels to receive packets
|
||||
osc_udp_server(ip, port, "osc_server")
|
||||
|
||||
# Associate Python functions with message address patterns
|
||||
osc_method("/joint_states", joint_states_handler, argscheme=osm.OSCARG_DATAUNPACK)
|
||||
|
||||
print(f"Listening for OSC messages on {ip}:{port}...")
|
||||
|
||||
try:
|
||||
# Run the event loop
|
||||
while True:
|
||||
osc_process() # Process OSC messages
|
||||
time.sleep(0.01) # Sleep to avoid high CPU usage
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("")
|
||||
|
||||
finally:
|
||||
# Properly close the system
|
||||
osc_terminate()
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,6 +1,6 @@
|
||||
AMENT_PREFIX_PATH=/BA/workspace/src/install/joint_info:/BA/workspace/src/install/joint_control:/opt/ros/humble
|
||||
AMENT_PREFIX_PATH=/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
|
||||
COLCON=1
|
||||
COLCON_PREFIX_PATH=/BA/workspace/src/install
|
||||
COLCON_PREFIX_PATH=/BA/workspace/install
|
||||
HOME=/root
|
||||
HOSTNAME=3230bc57d699
|
||||
LANG=C.UTF-8
|
||||
@@ -10,7 +10,7 @@ LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd
|
||||
OLDPWD=/BA/workspace/src
|
||||
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
|
||||
PWD=/BA/workspace/build/joint_info
|
||||
PYTHONPATH=/BA/workspace/src/build/joint_info:/BA/workspace/src/install/joint_info/lib/python3.10/site-packages:/BA/workspace/src/build/joint_control:/BA/workspace/src/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
PYTHONPATH=/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/BA/workspace/src/build/joint_info:/BA/workspace/src/install/joint_info/lib/python3.10/site-packages:/BA/workspace/src/build/joint_control:/BA/workspace/src/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
ROS_DISTRO=humble
|
||||
ROS_LOCALHOST_ONLY=0
|
||||
ROS_PYTHON_VERSION=3
|
||||
|
||||
@@ -1,13 +1,6 @@
|
||||
package.xml
|
||||
setup.cfg
|
||||
setup.py
|
||||
../../build/joint_info/joint_info.egg-info/PKG-INFO
|
||||
../../build/joint_info/joint_info.egg-info/SOURCES.txt
|
||||
../../build/joint_info/joint_info.egg-info/dependency_links.txt
|
||||
../../build/joint_info/joint_info.egg-info/entry_points.txt
|
||||
../../build/joint_info/joint_info.egg-info/requires.txt
|
||||
../../build/joint_info/joint_info.egg-info/top_level.txt
|
||||
../../build/joint_info/joint_info.egg-info/zip-safe
|
||||
joint_info/__init__.py
|
||||
joint_info/osc_joint_states_pub.py
|
||||
joint_info/osc_joint_states_sub.py
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user