Really bad state publisher server
Listens on port 9999 for 6 joint targets and a \n Angles are in degrees
This commit is contained in:
Thomas Muller
parent
9eff9ec145
commit
3c53174f63
4 changed files with 372 additions and 2 deletions
111
src/ar3_description/ar3_description/state_publisher_net.py
Normal file
111
src/ar3_description/ar3_description/state_publisher_net.py
Normal file
|
|
@ -0,0 +1,111 @@
|
|||
from math import sin, cos, pi, copysign
|
||||
import rclpy, random, time, threading
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile
|
||||
from geometry_msgs.msg import Quaternion
|
||||
from sensor_msgs.msg import JointState
|
||||
from tf2_ros import TransformBroadcaster, TransformStamped
|
||||
import socket
|
||||
|
||||
class StatePublisher(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('state_publisher')
|
||||
|
||||
self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
|
||||
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
self.sock.bind(('0.0.0.0', 9999))
|
||||
self.sock.listen(1)
|
||||
|
||||
qos_profile = QoSProfile(depth=10)
|
||||
self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
|
||||
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
|
||||
self.nodeName = self.get_name()
|
||||
self.get_logger().info("{0} started".format(self.nodeName))
|
||||
|
||||
self.cur_joints = [0.] * 6
|
||||
self.cmd_joints = [0.] * 6
|
||||
self.rate = 10. / 180. * pi
|
||||
self.accel = 10. / 180. * pi
|
||||
self.prev_time = time.time()
|
||||
|
||||
threading.Thread(target=self.server_handle).start()
|
||||
|
||||
def server_handle(self):
|
||||
while rclpy.ok():
|
||||
cli, addr = self.sock.accept()
|
||||
cli.settimeout(0)
|
||||
cli.setblocking(0)
|
||||
print(cli, addr)
|
||||
while rclpy.ok():
|
||||
try:
|
||||
msg = cli.recv(4096)
|
||||
if msg == b'':
|
||||
print('bailing')
|
||||
break
|
||||
# print(repr(msg))
|
||||
|
||||
if len(msg) >= 7 and msg[6] == 10:
|
||||
self.cmd_joints = [msg[0], msg[1], msg[2], msg[3], msg[4], msg[5]]
|
||||
self.cmd_joints = [a / 180. * pi for a in self.cmd_joints]
|
||||
except BlockingIOError as e:
|
||||
pass
|
||||
except Exception as e:
|
||||
print('what the fuck')
|
||||
print(e)
|
||||
raise e
|
||||
|
||||
cur_time = time.time()
|
||||
dt = cur_time - self.prev_time
|
||||
self.prev_time = cur_time
|
||||
# print(self.cmd_joints, self.cur_joints)
|
||||
# print(dt)
|
||||
|
||||
for i in range(6):
|
||||
self.cur_joints[i] += copysign(self.rate * dt + 0.5 * self.accel * dt * dt, self.cmd_joints[i] - self.cur_joints[i])
|
||||
|
||||
# message declarations
|
||||
odom_trans = TransformStamped()
|
||||
odom_trans.header.frame_id = 'odom'
|
||||
odom_trans.child_frame_id = 'base_link'
|
||||
joint_state = JointState()
|
||||
|
||||
# update joint_state
|
||||
now = self.get_clock().now()
|
||||
joint_state.header.stamp = now.to_msg()
|
||||
joint_state.name = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
|
||||
joint_state.position = self.cur_joints.copy()
|
||||
|
||||
# update transform
|
||||
# (moving in a circle with radius=2)
|
||||
odom_trans.header.stamp = now.to_msg()
|
||||
odom_trans.transform.translation.x = 0.0# cos(angle) * 0.4
|
||||
odom_trans.transform.translation.y = 0.0# sin(angle) * 0.4
|
||||
odom_trans.transform.translation.z = 0.0
|
||||
odom_trans.transform.rotation = \
|
||||
euler_to_quaternion(0, 0, 0) # roll,pitch,yaw
|
||||
|
||||
# send the joint state and transform
|
||||
self.joint_pub.publish(joint_state)
|
||||
self.broadcaster.sendTransform(odom_trans)
|
||||
|
||||
def euler_to_quaternion(roll, pitch, yaw):
|
||||
qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
|
||||
qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
|
||||
qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
|
||||
qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
|
||||
return Quaternion(x=qx, y=qy, z=qz, w=qw)
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
node = StatePublisher()
|
||||
|
||||
rclpy.spin(node)
|
||||
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
37
src/ar3_description/launch/net.launch.py
Normal file
37
src/ar3_description/launch/net.launch.py
Normal file
|
|
@ -0,0 +1,37 @@
|
|||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||
|
||||
urdf_file_name = 'ar3.urdf'
|
||||
urdf = os.path.join(
|
||||
get_package_share_directory('ar3_description'),
|
||||
urdf_file_name)
|
||||
with open(urdf, 'r') as infp:
|
||||
robot_desc = infp.read()
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time',
|
||||
default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
name='robot_state_publisher',
|
||||
output='screen',
|
||||
parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
|
||||
arguments=[urdf]),
|
||||
Node(
|
||||
package='ar3_description',
|
||||
executable='state_publisher_net',
|
||||
name='state_publisher_net',
|
||||
emulate_tty=True,
|
||||
output='screen'),
|
||||
])
|
||||
|
|
@ -7,7 +7,7 @@ package_name = 'ar3_description'
|
|||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.0',
|
||||
version='1.0.0',
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
|
|
@ -26,7 +26,8 @@ setup(
|
|||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'state_publisher = ar3_description.state_publisher:main'
|
||||
'state_publisher = ar3_description.state_publisher:main',
|
||||
'state_publisher_net = ar3_description.state_publisher_net:main'
|
||||
],
|
||||
},
|
||||
)
|
||||
|
|
|
|||
221
src/ar3_description/urdf/ar3.rviz
Normal file
221
src/ar3_description/urdf/ar3.rviz
Normal file
|
|
@ -0,0 +1,221 @@
|
|||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /TF1
|
||||
- /RobotModel1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 616
|
||||
- Class: rviz_common/Selection
|
||||
Name: Selection
|
||||
- Class: rviz_common/Tool Properties
|
||||
Expanded:
|
||||
- /2D Goal Pose1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz_common/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz_default_plugins/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base_link:
|
||||
Value: true
|
||||
link_1:
|
||||
Value: true
|
||||
link_2:
|
||||
Value: true
|
||||
link_3:
|
||||
Value: true
|
||||
link_4:
|
||||
Value: true
|
||||
link_5:
|
||||
Value: true
|
||||
link_6:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
odom:
|
||||
base_link:
|
||||
link_1:
|
||||
link_2:
|
||||
link_3:
|
||||
link_4:
|
||||
link_5:
|
||||
link_6:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_default_plugins/RobotModel
|
||||
Collision Enabled: false
|
||||
Description File: ""
|
||||
Description Source: Topic
|
||||
Description Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /robot_description
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link_1:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link_2:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link_3:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link_4:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link_5:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
link_6:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: base_link
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz_default_plugins/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz_default_plugins/MoveCamera
|
||||
- Class: rviz_default_plugins/Select
|
||||
- Class: rviz_default_plugins/FocusCamera
|
||||
- Class: rviz_default_plugins/Measure
|
||||
Line color: 128; 128; 0
|
||||
- Class: rviz_default_plugins/SetInitialPose
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /initialpose
|
||||
- Class: rviz_default_plugins/SetGoal
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /goal_pose
|
||||
- Class: rviz_default_plugins/PublishPoint
|
||||
Single click: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /clicked_point
|
||||
Transformation:
|
||||
Current:
|
||||
Class: rviz_default_plugins/TF
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz_default_plugins/Orbit
|
||||
Distance: 2.0058364868164062
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.0453979969024658
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 2.080399513244629
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002f3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e000002f3000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e000002f3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1200
|
||||
X: 1278
|
||||
Y: 80
|
||||
Loading…
Reference in a new issue