Merge pull request #11 from uuvsimulator/fix/set_ned_frame

FIX use_ned_frame was flag to NED inertial frame

rexrov2_control/launch/record_demo.launch

@@ -1,20 +1,39 @@
 <launch>
   <arg name="record" default="false"/>
   <arg name="bag_filename" default="$(find uuv_evaluation)/bags/recording.bag"/>
+  <arg name="use_ned_frame" default="false"/>
 
-  <node pkg="rosbag" type="record" name="recording"
-    args="record -O $(arg bag_filename)
-      /rexrov2/dp_controller/trajectory
-      /rexrov2/dp_controller/reference
-      /rexrov2/pose_gt
-      /hydrodynamics/current_velocity
-      /rexrov2/thruster_manager/input
-      /rexrov2/wrench_perturbation
-      /rexrov2/thrusters/0/thrust
-      /rexrov2/thrusters/1/thrust
-      /rexrov2/thrusters/2/thrust
-      /rexrov2/thrusters/3/thrust
-      /rexrov2/thrusters/4/thrust
-      /rexrov2/thrusters/5/thrust"
-    if="$(arg record)"/>
+  <group if="$(arg record)">
+    <node pkg="rosbag" type="record" name="recording"
+      args="record -O $(arg bag_filename)
+        /rexrov2/dp_controller/trajectory
+        /rexrov2/dp_controller/reference
+        /rexrov2/pose_gt_ned
+        /hydrodynamics/current_velocity
+        /rexrov2/thruster_manager/input
+        /rexrov2/wrench_perturbation
+        /rexrov2/thrusters/0/thrust
+        /rexrov2/thrusters/1/thrust
+        /rexrov2/thrusters/2/thrust
+        /rexrov2/thrusters/3/thrust
+        /rexrov2/thrusters/4/thrust
+        /rexrov2/thrusters/5/thrust"
+      if="$(arg use_ned_frame)"/>
+
+      <node pkg="rosbag" type="record" name="recording"
+        args="record -O $(arg bag_filename)
+          /rexrov2/dp_controller/trajectory
+          /rexrov2/dp_controller/reference
+          /rexrov2/pose_gt
+          /hydrodynamics/current_velocity
+          /rexrov2/thruster_manager/input
+          /rexrov2/wrench_perturbation
+          /rexrov2/thrusters/0/thrust
+          /rexrov2/thrusters/1/thrust
+          /rexrov2/thrusters/2/thrust
+          /rexrov2/thrusters/3/thrust
+          /rexrov2/thrusters/4/thrust
+          /rexrov2/thrusters/5/thrust"
+        unless="$(arg use_ned_frame)"/>
+  </group>
 </launch>

rexrov2_control/launch/start_mb_fl_controller.launch

@@ -4,6 +4,7 @@
     <arg name="record" default="false"/>
 
     <arg name="gui_on" default="true"/>
+    <arg name="use_ned_frame" default="false"/>
 
     <arg name="Kp" default="7750.307,7750.307,7750.307,7430.825,7430.825,7430.825"/>
     <arg name="Kd" default="8953.924,8953.924,8953.924,8267.137,8267.137,8267.137"/>
@@ -31,6 +32,7 @@
       <arg name="Kp" value="$(arg Kp)"/>
       <arg name="Kd" value="$(arg Kd)"/>
       <arg name="Ki" value="$(arg Ki)"/>
+      <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
       <arg name="thruster_manager_output_dir" value="$(arg thruster_manager_output_dir)"/>
       <arg name="thruster_manager_config_file" value="$(arg thruster_manager_config_file)"/>
       <arg name="tam_file" value="$(arg tam_file)"/>
@@ -40,6 +42,7 @@
     <!-- Initialize the recording fo the simulation according to the record flag  -->
     <include file="$(find rexrov2_control)/launch/record_demo.launch">
       <arg name="record" value="$(arg record)"/>
+      <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
     </include>
 
     <group if="$(arg teleop_on)">

rexrov2_control/launch/start_nmb_sm_controller.launch

@@ -4,6 +4,7 @@
     <arg name="record" default="false"/>
 
     <arg name="gui_on" default="true"/>
+    <arg name="use_ned_frame" default="false"/>
 
     <arg name="K" default="5,5,5,5,5,5"/>
     <arg name="Kd" default="4490.0,4490.0,4490.0,19541.395,19541.395,19541.395"/>
@@ -30,6 +31,7 @@
       <arg name="Kd" value="$(arg Kd)"/>
       <arg name="Ki" value="$(arg Ki)"/>
       <arg name="slope" value="$(arg slope)"/>
+      <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
       <arg name="thruster_manager_output_dir" value="$(arg thruster_manager_output_dir)"/>
       <arg name="thruster_manager_config_file" value="$(arg thruster_manager_config_file)"/>
       <arg name="tam_file" value="$(arg tam_file)"/>
@@ -38,6 +40,7 @@
     <!-- Initialize the recording fo the simulation according to the record flag  -->
     <include file="$(find rexrov2_control)/launch/record_demo.launch">
       <arg name="record" value="$(arg record)"/>
+      <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
     </include>
 
     <group if="$(arg teleop_on)">

rexrov2_control/launch/start_pid_controller.launch

@@ -4,6 +4,7 @@
     <arg name="record" default="false"/>
 
     <arg name="gui_on" default="true"/>
+    <arg name="use_ned_frame" default="false"/>
 
     <arg name="Kp" default="11993.888,11993.888,11993.888,19460.069,19460.069,19460.069"/>
     <arg name="Kd" default="9077.459,9077.459,9077.459,18880.925,18880.925,18880.925"/>
@@ -21,13 +22,15 @@
     <arg name="thruster_manager_config_file" default="$(find rexrov2_control)/config/thruster_manager.yaml"/>
     <arg name="tam_file" default="$(find rexrov2_control)/config/TAM.yaml"/>
 
+
     <include file="$(find uuv_trajectory_control)/launch/rov_pid_controller.launch">
       <arg name="uuv_name" value="$(arg uuv_name)"/>
       <arg name="gui_on" value="$(arg gui_on)"/>
       <arg name="saturation" value="2000"/>
       <arg name="Kp" value="$(arg Kp)"/>
       <arg name="Kd" value="$(arg Kd)"/>
       <arg name="Ki" value="$(arg Ki)"/>
+      <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
       <arg name="thruster_manager_output_dir" value="$(arg thruster_manager_output_dir)"/>
       <arg name="thruster_manager_config_file" value="$(arg thruster_manager_config_file)"/>
       <arg name="tam_file" value="$(arg tam_file)"/>
@@ -36,6 +39,7 @@
     <!-- Initialize the recording fo the simulation according to the record flag  -->
     <include file="$(find rexrov2_control)/launch/record_demo.launch">
       <arg name="record" value="$(arg record)"/>
+      <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
     </include>
 
     <group if="$(arg teleop_on)">

rexrov2_control/launch/start_sf_controller.launch

@@ -0,0 +1,66 @@
+<launch>
+    <arg name="uuv_name" default="rexrov2"/>
+    <!-- Flag to start recoding a rosbag  -->
+    <arg name="record" default="false"/>
+
+    <arg name="gui_on" default="true"/>
+
+    <arg name="Kd" default="100.0,100.0,100.0,100.0,100.0,100.0"/>
+    <arg name="delta" default="10.0,10.0,10.0,10.0,10.0,10.0"/>
+
+    <arg name="teleop_on" default="false"/>
+    <arg name="joy_id" default="0"/>
+    <arg name="axis_yaw" default="0"/>
+    <arg name="axis_x" default="4"/>
+    <arg name="axis_y" default="3"/>
+    <arg name="axis_z" default="1"/>
+
+    <!-- Vehicle parameter for the controller -->
+    <arg name="model_params_file" default="$(find rexrov2_control)/config/model_params.yaml"/>
+
+    <!-- Thruster manager configuration -->
+    <arg name="thruster_manager_output_dir" default="$(find rexrov2_control)/config"/>
+    <arg name="thruster_manager_config_file" default="$(find rexrov2_control)/config/thruster_manager.yaml"/>
+    <arg name="tam_file" default="$(find rexrov2_control)/config/TAM.yaml"/>
+
+    <include file="$(find uuv_trajectory_control)/launch/rov_sf_controller.launch">
+      <arg name="uuv_name" value="$(arg uuv_name)"/>
+      <arg name="gui_on" value="$(arg gui_on)"/>
+      <arg name="saturation" value="2000"/>
+      <arg name="Kd" value="$(arg Kd)"/>
+      <arg name="delta" value="$(arg delta)"/>
+      <arg name="thruster_manager_output_dir" value="$(arg thruster_manager_output_dir)"/>
+      <arg name="thruster_manager_config_file" value="$(arg thruster_manager_config_file)"/>
+      <arg name="tam_file" value="$(arg tam_file)"/>
+      <arg name="model_params_file" value="$(arg model_params_file)"/>
+    </include>
+
+    <!-- Initialize the recording fo the simulation according to the record flag  -->
+    <include file="$(find rexrov2_control)/launch/record_demo.launch">
+      <arg name="record" value="$(arg record)"/>
+      <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
+    </include>
+
+    <group if="$(arg teleop_on)">
+      <!-- Start joystick teleop node  -->
+      <include file="$(find uuv_teleop)/launch/uuv_teleop.launch">
+          <arg name="uuv_name" value="$(arg uuv_name)"/>
+          <arg name="joy_id" value="$(arg joy_id)"/>
+          <arg name="output_topic" value="cmd_vel"/>
+          <arg name="message_type" value="twist"/>
+          <arg name="axis_yaw" value="$(arg axis_yaw)"/>
+          <arg name="axis_x" value="$(arg axis_x)"/>
+          <arg name="axis_y" value="$(arg axis_y)"/>
+          <arg name="axis_z" value="$(arg axis_z)"/>
+          <arg name="gain_yaw" default="0.2"/>
+          <arg name="gain_x" default="0.5"/>
+          <arg name="gain_y" default="0.5"/>
+          <arg name="gain_z" default="0.5"/>
+      </include>
+    </group>
+
+    <group if="$(arg gui_on)">
+      <!-- Start RViz  -->
+      <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find rexrov2_control)/rviz/rexrov2_control.rviz"/>
+    </group>
+</launch>

rexrov2_description/launch/upload_rexrov2.launch

@@ -11,15 +11,26 @@
   <arg name="mode" default="default"/>
   <arg name="namespace" default="rexrov2"/>
   <arg name="use_simplified_mesh" default="false"/>
-  <arg name="world_frame" default="world"/>
+  <arg name="use_ned_frame" default="false"/>
 
   <group ns="$(arg namespace)">
-    <param name="robot_description"
-           command="$(find xacro)/xacro '$(find rexrov2_description)/robots/rexrov2_$(arg mode).xacro' --inorder
-                    use_simplified_mesh:=$(arg use_simplified_mesh)
-                    debug:=$(arg debug)
-                    namespace:=$(arg namespace)
-                    inertial_reference_frame:=$(arg world_frame)" />
+    <group if="$(arg use_ned_frame)">
+      <param name="robot_description"
+             command="$(find xacro)/xacro '$(find rexrov2_description)/robots/rexrov2_$(arg mode).xacro' --inorder
+                      use_simplified_mesh:=$(arg use_simplified_mesh)
+                      debug:=$(arg debug)
+                      namespace:=$(arg namespace)
+                      inertial_reference_frame:=world_ned" />
+    </group>
+
+    <group unless="$(arg use_ned_frame)">
+      <param name="robot_description"
+             command="$(find xacro)/xacro '$(find rexrov2_description)/robots/rexrov2_$(arg mode).xacro' --inorder
+                      use_simplified_mesh:=$(arg use_simplified_mesh)
+                      debug:=$(arg debug)
+                      namespace:=$(arg namespace)
+                      inertial_reference_frame:=world" />
+    </group>
 
     <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
     <node name="urdf_spawner" pkg="uuv_descriptions" type="spawn_model.py" respawn="false" output="screen"
@@ -34,13 +45,10 @@
     </node>
   </group>
 
-  <include file="$(find uuv_assistants)/launch/publish_body_sname.launch">
-    <arg name="uuv_name" value="$(arg namespace)"/>
-  </include>
-
   <include file="$(find uuv_assistants)/launch/message_to_tf.launch">
     <arg name="namespace" value="$(arg namespace)"/>
-    <arg name="world_frame" value="$(arg world_frame)"/>
+    <arg name="world_frame" value="world"/>
+    <arg name="child_frame_id" value="/$(arg namespace)/base_link"/>
   </include>
 
 </launch>

rexrov2_gazebo/launch/start_demo_mb_fl_controller.launch

@@ -0,0 +1,40 @@
+<launch>
+    <arg name="record" default="false"/>
+    <arg name="bag_filename" default="recording.bag"/>
+    <arg name="use_ned_frame" default="false"/>
+
+    <!-- Vehicle's initial position -->
+    <arg name="x" default="0"/>
+    <arg name="y" default="0"/>
+    <arg name="z" default="-5"/>
+    <arg name="yaw" default="0"/>
+
+    <arg name="teleop_on" default="false"/>
+    <arg name="joy_id" default="0"/>
+
+    <include file="$(find uuv_descriptions)/launch/ocean_waves.launch"/>
+
+    <include file="$(find uuv_simulation_wrapper)/launch/unpause_simulation.launch">
+        <arg name="timeout" value="5"/>
+    </include>
+
+    <include file="$(find rexrov2_description)/launch/upload_rexrov2.launch">
+        <arg name="x" value="$(arg x)"/>
+        <arg name="y" value="$(arg y)"/>
+        <arg name="z" value="$(arg z)"/>
+        <arg name="yaw" value="$(arg yaw)"/>
+        <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
+    </include>
+
+    <include file="$(find rexrov2_control)/launch/start_mb_fl_controller.launch">
+        <arg name="teleop_on" value="$(arg teleop_on)"/>
+        <arg name="joy_id" value="$(arg joy_id)"/>
+        <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
+    </include>
+
+    <include file="$(find rexrov2_gazebo)/launch/record.launch">
+      <arg name="record" value="$(arg record)"/>
+      <arg name="bag_filename" value="$(arg bag_filename)"/>
+    </include>
+
+</launch>

rexrov2_gazebo/launch/start_demo_nmb_sm_controller.launch

@@ -1,6 +1,7 @@
 <launch>
     <arg name="record" default="false"/>
     <arg name="bag_filename" default="recording.bag"/>
+    <arg name="use_ned_frame" default="false"/>
 
     <!-- Vehicle's initial position -->
     <arg name="x" default="0"/>
@@ -11,16 +12,24 @@
     <arg name="teleop_on" default="false"/>
     <arg name="joy_id" default="0"/>
 
+    <include file="$(find uuv_descriptions)/launch/ocean_waves.launch"/>
+
+    <include file="$(find uuv_simulation_wrapper)/launch/unpause_simulation.launch">
+        <arg name="timeout" value="5"/>
+    </include>
+
     <include file="$(find rexrov2_description)/launch/upload_rexrov2.launch">
         <arg name="x" value="$(arg x)"/>
         <arg name="y" value="$(arg y)"/>
         <arg name="z" value="$(arg z)"/>
         <arg name="yaw" value="$(arg yaw)"/>
+        <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
     </include>
 
     <include file="$(find rexrov2_control)/launch/start_nmb_sm_controller.launch">
         <arg name="teleop_on" value="$(arg teleop_on)"/>
         <arg name="joy_id" value="$(arg joy_id)"/>
+        <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
     </include>
 
     <include file="$(find rexrov2_gazebo)/launch/record.launch">

rexrov2_gazebo/launch/start_demo_pid_controller.launch

@@ -1,7 +1,8 @@
 <launch>
     <arg name="record" default="false"/>
     <arg name="bag_filename" default="recording.bag"/>
-
+    <arg name="use_ned_frame" default="false"/>
+
     <!-- Vehicle's initial position -->
     <arg name="x" default="0"/>
     <arg name="y" default="0"/>
@@ -11,16 +12,24 @@
     <arg name="teleop_on" default="false"/>
     <arg name="joy_id" default="0"/>
 
+    <include file="$(find uuv_descriptions)/launch/ocean_waves.launch"/>
+
+    <include file="$(find uuv_simulation_wrapper)/launch/unpause_simulation.launch">
+        <arg name="timeout" value="5"/>
+    </include>
+
     <include file="$(find rexrov2_description)/launch/upload_rexrov2.launch">
         <arg name="x" value="$(arg x)"/>
         <arg name="y" value="$(arg y)"/>
         <arg name="z" value="$(arg z)"/>
         <arg name="yaw" value="$(arg yaw)"/>
+        <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
     </include>
 
     <include file="$(find rexrov2_control)/launch/start_pid_controller.launch">
         <arg name="teleop_on" value="$(arg teleop_on)"/>
         <arg name="joy_id" value="$(arg joy_id)"/>
+        <arg name="use_ned_frame" value="$(arg use_ned_frame)"/>
     </include>
 
     <include file="$(find rexrov2_gazebo)/launch/record.launch">