Merge pull request #6 from ethz-msrl/minor_fix

Minor fix
ethz-msrl · Oct 27, 2023 · 8fd7e76 · 8fd7e76
2 parents 4d7dc82 + 71341b6
commit 8fd7e76
Show file tree

Hide file tree

Showing 43 changed files with 1,472 additions and 11 deletions.
diff --git a/desktop/default/navion2_desktop.desktop b/desktop/default/navion2_desktop.desktop
@@ -3,7 +3,7 @@ Version=1.0
 Type=Application
 Terminal=true
 TerminalOptions=bash
-Exec=bash -c "killall -9 rosmaster; source /opt/ros/noetic/setup.bash;source ~/navion_ws/install/setup.bash;source ~/catkin_ws/devel/setup.bash; roslaunch navion_example navion_desktop.launch"
-Name=Navion Rviz
-Comment=Launch Navion software with RViz GUI, cart controller, and PS5 joystick magnetic field control.
+Exec=bash -c "killall -9 rosmaster; source /opt/ros/noetic/setup.bash;source ~/navion_ws/install/setup.bash;source ~/catkin_ws/devel/setup.bash;roslaunch navion_example navion_desktop.launch"
+Name=Navion Desktop
+Comment=Launch Navion software with RViz GUI, cart controller, and PS5 joystick field control.
 Icon=/home/<username>/.local/share/icons/hicolor/48x48/apps/navion_logo.png
diff --git a/desktop/default/navion2_rviz_rotating_fields_joystick.desktop b/desktop/default/navion2_rviz_rotating_fields_joystick.desktop
@@ -0,0 +1,9 @@
+[Desktop Entry]
+Version=1.0
+Type=Application
+Terminal=true
+TerminalOptions=bash
+Exec=bash -c "killall -9 rosmaster; source /opt/ros/noetic/setup.bash;source ~/navion_ws/install/setup.bash;source ~/catkin_ws/devel/setup.bash;roslaunch navion_example navion_rotating_field_joystick.launch"
+Name=Navion Fast Rotating Fields with PS5 controller
+Comment=Launch Navion software with RViz GUI.
+Icon=/home/<username>/.local/share/icons/hicolor/48x48/apps/navion_logo.png
diff --git a/navion_example/calib/cam_demo.yaml b/navion_example/calib/cam_demo.yaml
@@ -0,0 +1,26 @@
+image_width: 400
+image_height: 400
+camera_name: narrow_stereo
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [261.24689,   0.     , 186.49292,
+           0.     , 260.77928, 195.48004,
+           0.     ,   0.     ,   1.     ]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.181739, 0.016756, -0.000477, -0.000213, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1., 0., 0.,
+         0., 1., 0.,
+         0., 0., 1.]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [225.43288,   0.     , 180.6873 ,   0.     ,
+           0.     , 225.59071, 192.84695,   0.     ,
+           0.     ,   0.     ,   1.     ,   0.     ]
diff --git a/navion_example/config/ps_controller_rotating_field.yaml b/navion_example/config/ps_controller_rotating_field.yaml
@@ -0,0 +1,35 @@
+controller_name: ps_navi
+eMNS_name: navion
+
+joy_node/dev: "/dev/input/psc_5"
+
+###### CHANGE CONTROLLER MAPPING ###############################
+# mapping 0: joystick axis 0 -> rot(X),  axis 1 -> rot(Y)      #
+# mapping 1: joystick axis 0 -> azimuth, axis 1 -> inclination #
+# mapping 2: joystick axis 0 -> none,    axis 1 -> rot(Y)      #
+# mapping 3: absolute field mapping x-z plane                  #
+/ps_navi_controller/mapping: 1 #mapping for the rotations      #
+#                                                              #
+################################################################
+
+/ps_navi_controller/mfControlMode: 1 #control mode for the magnetic field
+
+/ps_navi_controller/joy_axis_0: 1
+/ps_navi_controller/joy_axis_1: 0
+/ps_navi_controller/joy_axis_l1: 3
+
+/ps_navi_controller/button_x: 1
+/ps_navi_controller/button_o: 0
+/ps_navi_controller/button_up: 11
+/ps_navi_controller/button_down: 12
+/ps_navi_controller/button_left: 10
+/ps_navi_controller/button_right: 11
+/ps_navi_controller/button_ps: 6
+/ps_navi_controller/button_l1: 5
+/ps_navi_controller/button_l2: 4
+/ps_navi_controller/button_l3: 7
+
+/ps_navi_controller/angleIncrement: 0.040
+/ps_navi_controller/posIncrement: 0.0005
+/ps_navi_controller/mfMagnitude: 0.01
+/ps_navi_controller/maxMfMagnitudeIncrement_: 0.0001
diff --git a/navion_example/launch/navion_cam.launch b/navion_example/launch/navion_cam.launch
@@ -1,22 +1,25 @@
 <?xml version="1.0"?>
 <launch>
+
    <!-- launch video stream -->
    <include file="$(find video_stream_opencv)/launch/camera.launch" >
    		<!-- node name and ros graph name -->
-	  	<arg name="camera_name" value="camera" />
+	  	<arg name="camera_name" value="camera_tne" />
 	  	<!-- means video device 0, /dev/video0 -->
-	  	<arg name="video_stream_provider" value="4" />
+	  	<arg name="video_stream_provider" value="0" />
 	  	<!-- set camera fps to (if the device allows) -->
 	  	<arg name="set_camera_fps" value="30"/>
       	<!-- set buffer queue size of frame capturing to (1 means we want the latest frame only) -->
       	<arg name="buffer_queue_size" value="1" />
 	  	<!-- throttling the querying of frames to -->
 	  	<arg name="fps" value="30" />
 	  	<!-- setting frame_id -->
-	  	<arg name="frame_id" value="webcam_optical_frame" />
+	  	<arg name="frame_id" value="camera" />
 	  	<!-- camera info loading, take care as it needs the "file:///" at the start , e.g.:
 	  	"file:///$(find your_camera_package)/config/your_camera.yaml" -->
-	  	<arg name="camera_info_url" value="" />
+	  	<arg 
+			name="camera_info_url"
+			value="file:///$(find navion_example)/calib/cam_demo.yaml" />
 	  	<!-- flip the image horizontally (mirror it) -->
 	  	<arg name="flip_horizontal" value="false" />
 	  	<!-- flip the image vertically -->
@@ -29,4 +32,11 @@
 	  	<!-- visualize on an image_view window the stream generated -->
 	  	<arg name="visualize" value="false" />
    </include>
+
+    <node 
+        pkg="tf" 
+        type="static_transform_publisher" 
+        name="camera_broadcaster" 
+        args="0 0 0 0 0 0 1 mns camera 100" />
+
 </launch>
diff --git a/navion_example/launch/navion_pov.launch b/navion_example/launch/navion_pov.launch
diff --git a/navion_example/launch/navion_psc.launch b/navion_example/launch/navion_psc.launch
@@ -2,14 +2,23 @@
 
 <launch>  
 
-    <!-- PS Navi controller -->
     <rosparam command="load" file="$(find navion_example)/config/ps_controller.yaml" />
 
     <node respawn="true" pkg="joy" type="joy_node" name="joy_node">
             <param name="autorepeat_rate" value="50"/>
     </node>
+
+     <!-- Field control -->
     <node pkg="ps_navi_controller" type="ps_navi_controller" name="ps_navi_controller" respawn="false">
         <remap from="/ps_navi_controller/target_mf" to="/backward_model/field"/>
     </node>
 
+    <!-- Fluid box control -->
+    <node
+        pkg="endoscopy_cart_joystick"
+        type="ps_controller_digout"
+        name="ps_controller_digout" 
+        output="screen"/>
+
+
 </launch>
diff --git a/navion_example/launch/navion_psc_rotating.launch b/navion_example/launch/navion_psc_rotating.launch
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<launch>  
+
+    <!-- PS Navi controller -->
+    <rosparam command="load" file="$(find navion_example)/config/ps_controller_rotating_field.yaml" />
+
+    <node respawn="true" pkg="joy" type="joy_node" name="joy_node">
+            <param name="autorepeat_rate" value="50"/>
+    </node>
+    <node pkg="ps_navi_controller" type="ps_navi_controller" name="ps_navi_controller" respawn="false">
+        <remap from="/ps_navi_controller/target_mf" to="/navion/desired_field_rot_axis"/>
+    </node>
+
+</launch>
diff --git a/navion_example/launch/navion_rotating_field_joystick.launch b/navion_example/launch/navion_rotating_field_joystick.launch
@@ -0,0 +1,29 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<launch>  
+
+    <!-- Navion -->
+        <arg name="emns_param_path" default="$(find mag_manip)/params/Navion_2.yaml" />
+    <arg name="navion_version" default="2_1" />
+    <arg name="cal_path" default="$(find mpem)/cal/Navion_2-1_Calibration_25-05-2022.yaml" />
+
+    <node name="mns_broadcaster" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 1 world mns 100" respawn="true" />
+
+    <node name="nav_controller" pkg="nav_controller" type="navion2_controller_node" output="screen" respawn="true">
+        <rosparam command="load" file="$(find nav_controller)/launch/parameters_fast_fields.yaml" />
+        <rosparam command="load" file="$(find nav_controller)/param/navion_$(arg navion_version).yaml" />
+        <param name="emns_param" type="string" value="$(find mag_manip)/params/Navion_2.yaml"/>
+    </node>
+
+    <node name="rviz" pkg="rviz" type='rviz' args="-d $(find navion_example)/rviz/config_navion_rotating.rviz" output="screen" />
+
+    <node name="rotating_field_node" pkg="nav_utils" type='rotating_field_node.py' output="screen" respawn="true">
+         <param name="~calibration_path" type="string" value="$(arg cal_path)" />
+    </node>
+
+    <include file="$(find nav_rviz)/launch/navion2_urdf_model.launch"/>
+
+    <!-- PlayStation controller -->
+    <include file="$(find navion_example)/launch/navion_psc_rotating.launch" pass_all_args="true" />
+
+</launch>
diff --git a/navion_example/package.xml b/navion_example/package.xml
@@ -20,7 +20,7 @@
     limitations under the License.
 -->
 
-<package>
+<package format="3">
   <name>navion_example</name>
   <version>3.1.0</version>
   <description>The navion example package</description>
@@ -34,7 +34,10 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>tf</build_depend>
 
-  <run_depend>video_stream_opencv</run_depend>
-  <run_depend>nav_launch</run_depend>
+  <depend>endoscopy_cart_joystick</depend>
+  <depend>endoscopy_cart_controller</depend>
+
+  <depend>video_stream_opencv</depend>
+  <depend>nav_launch</depend>
 
 </package>