Merge pull request #41 from iory/mesh

Enable simple viewer for any kxr
iory · Feb 28, 2024 · 0b553f9 · 0b553f9
2 parents 1dff6ab + 7a142b1
commit 0b553f9
Show file tree

Hide file tree

Showing 22 changed files with 604 additions and 49 deletions.
diff --git a/ros/kxr_controller/launch/kxr_controller.launch b/ros/kxr_controller/launch/kxr_controller.launch
@@ -8,6 +8,12 @@
   <arg name="control_loop_rate" default="20" />
   <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
   <arg name="device" default="" doc="Device path"/>
+  <arg name="model_server_port" default="8123" />
+
+  <param name="/model_server_port" value="$(arg model_server_port)" />
+  <node name="http_server_node"
+        pkg="kxr_models" type="http_server_node.py" >
+  </node>
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >
     <param name="control_loop_rate" value="$(arg control_loop_rate)" />
@@ -25,7 +31,9 @@
         use_rcb4: $(arg use_rcb4)
       </rosparam>
     </node>
-
+    <node name="urdf_model_server"
+          pkg="kxr_models" type="urdf_model_server.py" >
+    </node>
   </group>
 
   <group unless="$(eval len(arg('namespace')) > 0)">
@@ -45,6 +53,9 @@
       </rosparam>
     </node>
 
+    <node name="urdf_model_server"
+          pkg="kxr_models" type="urdf_model_server.py" >
+    </node>
   </group>
 
 </launch>
diff --git a/ros/kxr_controller/launch/kxr_controller_for_wheel.launch b/ros/kxr_controller/launch/kxr_controller_for_wheel.launch
@@ -4,12 +4,14 @@
   <arg name="servo_config_path" default="$(find kxr_models)/config/kxrmw4a6h2m_servo_config.yaml" />
   <arg name="use_rcb4" default="false" doc="Flag to use RCB4 mini board"/>
   <arg name="device" default="" doc="Device path"/>
+  <arg name="model_server_port" default="8123" />
 
   <include file="$(find kxr_controller)/launch/kxr_controller.launch" >
     <arg name="urdf_path" value="$(arg urdf_path)" />
     <arg name="servo_config_path" value="$(arg servo_config_path)" />
     <arg name="use_rcb4" value="$(arg use_rcb4)" />
     <arg name="device" value="$(arg device)" />
+    <arg name="model_server_port" value="$(arg model_server_port)" />
   </include>
 
   <include file="$(find kxr_controller)/launch/diff_drive_controller.launch" />

diff --git a/ros/kxr_controller/launch/rviz/viewer.rviz b/ros/kxr_controller/launch/rviz/viewer.rviz
@@ -0,0 +1,251 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/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
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        HEAD_LINK0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HEAD_LINK1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_LINK0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_LINK1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_LINK2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_LINK3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_LINK4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_LINK5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_LINK6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LARM_LINK7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LLEG_LINK0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LLEG_LINK1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        RARM_LINK0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_LINK1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_LINK2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_LINK3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_LINK4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_LINK5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_LINK6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RARM_LINK7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RLEG_LINK0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RLEG_LINK1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bodyset94472077639384:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description_viz
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2.9285101890563965
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      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: 0.6703981161117554
+      Target Frame: <Fixed Frame>
+      Yaw: 0.510398268699646
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000169000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000022c000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 368
+  Y: 214
diff --git a/ros/kxr_controller/launch/viewer.launch b/ros/kxr_controller/launch/viewer.launch
@@ -0,0 +1,17 @@
+<launch>
+
+  <arg name="connect_tf" default="true" />
+
+  <node name="get_urdf_model"
+        pkg="kxr_models" type="get_urdf_model.py" >
+    <rosparam subst_value="true">
+      connect_tf: $(arg connect_tf)
+    </rosparam>
+  </node>
+
+  <node name="$(anon rviz)"
+       pkg="rviz" type="rviz"
+       args="-d $(find kxr_controller)/launch/rviz/viewer.rviz" >
+  </node>
+
+</launch>
diff --git a/ros/kxr_controller/package.xml b/ros/kxr_controller/package.xml
@@ -33,6 +33,7 @@
   <exec_depend>urdf</exec_depend>
   <exec_depend>cmake_modules</exec_depend>
   <exec_depend>kxr_models</exec_depend>
+  <exec_depend>rviz</exec_depend>
 
   <test_depend>roslaunch</test_depend>
   <test_depend>rostest</test_depend>

diff --git a/ros/kxr_controller/requirements.in b/ros/kxr_controller/requirements.in
@@ -1,2 +1,3 @@
+numpy==1.24.4
 pyglet==1.4.10
 scikit-robot>=0.0.37
diff --git a/ros/kxr_models/CMakeLists.txt b/ros/kxr_models/CMakeLists.txt
@@ -4,8 +4,15 @@ project(kxr_models)
 
 find_package(catkin REQUIRED)
 
+catkin_python_setup()
+
 catkin_package()
 
+file(GLOB PYTHON_SCRIPT_FILES scripts/* node_scripts/*)
+catkin_install_python(
+  PROGRAMS ${PYTHON_SCRIPT_FILES}
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
 find_package(roslaunch)
 foreach(dir config meshes urdf)
 	install(DIRECTORY ${dir}/

diff --git a/ros/kxr_models/models/.gitignore b/ros/kxr_models/models/.gitignore
@@ -0,0 +1,2 @@
+*
+!.gitignore
diff --git a/ros/kxr_models/models/urdf/.gitignore b/ros/kxr_models/models/urdf/.gitignore
@@ -0,0 +1,2 @@
+*
+!.gitignore
diff --git a/ros/kxreus/node_scripts/http_server_node.py → ...r_models/node_scripts/http_server_node.py b/ros/kxreus/node_scripts/http_server_node.py → ...r_models/node_scripts/http_server_node.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 from http.server import HTTPServer
 from http.server import SimpleHTTPRequestHandler
@@ -63,15 +63,10 @@ def stop(self):
     rospy.init_node('http_server_node')
 
     rospack = rospkg.RosPack()
-    kxreus_path = rospack.get_path('kxreus')
+    kxreus_path = rospack.get_path('kxr_models')
     www_directory = os.path.join(kxreus_path, 'models')
 
-    full_namespace = rospy.get_namespace()
-    last_slash_pos = full_namespace.rfind('/')
-    clean_namespace = full_namespace[:last_slash_pos] \
-        if last_slash_pos != 0 else ''
-
-    port = rospy.get_param(clean_namespace + '/port', 8123)
+    port = rospy.get_param('/model_server_port', 8123)
     server = ThreadedHTTPServer(
         '0.0.0.0', port, CustomHTTPRequestHandler, www_directory)
     server.start()