add rosbag to launch file and modify visualization tool

JuliaMPC · Dec 4, 2018 · 23645be · 23645be
1 parent 4fd7d72
commit 23645be
Show file tree

Hide file tree

Showing 7 changed files with 46 additions and 13 deletions.
diff --git a/ros/src/computing/tools/visualization/data_plotter.py b/ros/src/computing/tools/visualization/data_plotter.py
@@ -68,8 +68,8 @@ def __init__(self):
         self.t_band = 10
         self.x_band = 20
         self.y_band = 50
-        self.sa_band = 0.2
-        self.ux_band = 20
+        self.sa_band = 0.4
+        self.ux_band = 24
         self.ax_band = 5
         self.solve_num_band = 20
 
@@ -137,12 +137,12 @@ def __init__(self):
         self.tSolve_subplot.add_line(self.line_solve_time_pass)
         self.tSolve_subplot.add_line(self.line_solve_time_fail)
         self.tSolve_subplot.add_line(self.line_solve_time_limit)
-        self.tSolve_subplot.legend(['optimal', 'non-optimal', 'solve time limit'], fontsize='x-small')
-        self.tSolve_subplot.set_xlabel('number of iterations', fontsize='larger', verticalalignment='center')
+        self.tSolve_subplot.legend(['optimal', 'non-optimal', 'target maximum solve time'], fontsize='x-small')
+        self.tSolve_subplot.set_xlabel('iterations', fontsize='larger', verticalalignment='center')
         self.tSolve_subplot.set_ylabel('solve time (s)', fontsize='larger')
         self.tSolve_subplot.tick_params('both', labelsize='x-small')
         self.tSolve_subplot.grid(True)
-        self.tSolve_subplot.set_xlim(0,self.solve_num_band)
+        self.tSolve_subplot.set_xlim(1,self.solve_num_band)
         self.tSolve_subplot.set_ylim(0,1.0)
 
     def draw_frame(self):
@@ -163,7 +163,7 @@ def draw_frame(self):
             self.line_ux_traj.set_data(self.t_traj, self.ux_traj)
             self.line_ux_traj_last.set_data(self.t_traj_last, self.ux_traj_last)
             if (len(self.ux_actual)):
-                self.ux_subplot.set_ylim(self.ux_actual[-1]-self.ux_band/2, self.ux_actual[-1]+self.ux_band/2)
+                self.ux_subplot.set_ylim(self.ux_actual[-1]-self.ux_band*3/4, self.ux_actual[-1]+self.ux_band*1/4)
 
             self.line_ax_actual.set_data(self.t_actual, self.ax_actual)
             self.line_ax_traj.set_data(self.t_traj, self.ax_traj)
@@ -176,12 +176,12 @@ def draw_frame(self):
 
             # moving time axis
             if (len(self.t_actual) > 0 and self.t_actual[-1] > 10):
-                self.sa_subplot.set_xlim(self.t_actual[-1]-self.t_band*3/4, self.t_actual[-1]+self.t_band/4)
-                self.ux_subplot.set_xlim(self.t_actual[-1]-self.t_band*3/4, self.t_actual[-1]+self.t_band/4)
-                self.ax_subplot.set_xlim(self.t_actual[-1]-self.t_band*3/4, self.t_actual[-1]+self.t_band/4)
+                self.sa_subplot.set_xlim(self.t_actual[-1]-self.t_band, self.t_actual[-1])
+                self.ux_subplot.set_xlim(self.t_actual[-1]-self.t_band, self.t_actual[-1])
+                self.ax_subplot.set_xlim(self.t_actual[-1]-self.t_band, self.t_actual[-1])
 
-            if (len(self.solve_num) > 0 and self.solve_num[-1] > 20):
-                self.tSolve_subplot.set_xlim(self.t_actual[-1], self.t_actual[-1]+self.solve_num_band)
+            if (len(self.solve_num) > 0 and self.solve_num[-1] > self.solve_num_band):
+                self.tSolve_subplot.set_xlim(self.solve_num[0], self.solve_num[-1])
 
 
             plt.draw()

diff --git a/ros/src/computing/tools/visualization/playback.launch b/ros/src/computing/tools/visualization/playback.launch
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
-
 <launch>
+  <arg name="ros_bag_name" default="demoI"/>
 
   <node name="visualization" pkg="visualization" type="data_plotter.py" />
-  <node pkg="rosbag" type="play" name="record" args="/home/mavs/MAVs/results/demoI.bag"/>
+  <node pkg="rosbag" type="play" name="record" args="/home/mavs/MAVs/results/$(arg ros_bag_name).bag"/>
 
 </launch>
diff --git a/ros/src/system/config/case/s4.yaml b/ros/src/system/config/case/s4.yaml
@@ -9,8 +9,11 @@ case:
   X0:
    x: 200.0
    yVal: 0.0
+   z: 0.5
    v: 0.0
    r: 0.0
+   phi: 0.0
+   theta: 0.0
    psi: 1.57079632679
    sa: 0.0
    ux: 17.0

diff --git a/ros/src/system/config/case/s9.yaml b/ros/src/system/config/case/s9.yaml
@@ -9,8 +9,11 @@ case:
   X0:
    x: 6.0
    yVal: 0.0
+   z: 0.5
    v: 0.0
    r: 0.0
+   phi: 0.0
+   theta: 0.0
    psi: 1.57079632679
    sa: 0.0
    ux: 17.0

diff --git a/ros/src/system/launch/demoI.launch b/ros/src/system/launch/demoI.launch
@@ -21,6 +21,9 @@
   <!-- use Gazebo's simulation time -->
   <param name="use_sim_time" value="true"/>
 
+  <!-- rosbag arg -->
+  <arg name="ros_bag_name" default="demoI"/>
+
   <!--  ************** load args and parameters ***************  -->
 
   <!--  ************** launch scripts ***************  -->
@@ -58,6 +61,12 @@
   <!--display trajectory-->
   <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find obstacle_detector)/resources/display.rviz"/>
 
+  <!-- save data using rosbag -->
+  <node pkg="rosbag" type="record" name="record" args="record -O /home/mavs/MAVs/results/$(arg ros_bag_name).bag 
+                                                                 /nlopcontrol_planner/opt
+                                                                 /nlopcontrol_planner/control
+                                                                 /state"/>
+
   <!--system_shutdown node-->
   <node name="system_shutdown" pkg="system" type="system_shutdown" output="screen" required="true">
      <rosparam file="$(arg system_params_path)" command="load"/>

diff --git a/ros/src/system/launch/demoK.launch b/ros/src/system/launch/demoK.launch
@@ -21,6 +21,9 @@
   <!-- use Gazebo's simulation time -->
   <param name="use_sim_time" value="true"/>
 
+  <!-- rosbag arg -->
+  <arg name="ros_bag_name" default="demoK"/>
+
   <!--  ************** load args and parameters ***************  -->
 
   <!--  ************** launch scripts ***************  -->
@@ -63,6 +66,12 @@
      <rosparam file="$(arg system_params_path)" command="load"/>
   </node>
 
+  <!-- save data using rosbag -->
+  <node pkg="rosbag" type="record" name="record" args="record -O /home/mavs/MAVs/results/$(arg ros_bag_name).bag 
+                                                                 /nlopcontrol_planner/opt
+                                                                 /nlopcontrol_planner/control
+                                                                 /state"/>
+
   <node name="visualization" pkg="visualization" type="data_plotter.py" />
   <!--  ************** additional nodes ***************  -->
 

diff --git a/ros/src/system/launch/demoM.launch b/ros/src/system/launch/demoM.launch
@@ -49,6 +49,9 @@
     <arg name="manager" value="$(arg manager)" />
   </include>
 
+  <!-- rosbag arg -->
+  <arg name="ros_bag_name" default="demoM"/>
+
   <!--  ************** launch scripts ***************  -->
 
 
@@ -72,6 +75,12 @@
      <rosparam file="$(arg system_params_path)" command="load"/>
   </node>
 
+  <!-- save data using rosbag -->
+  <node pkg="rosbag" type="record" name="record" args="record -O /home/mavs/MAVs/results/$(arg ros_bag_name).bag 
+                                                                 /nlopcontrol_planner/opt
+                                                                 /nlopcontrol_planner/control
+                                                                 /state"/>
+
   <node name="visualization" pkg="visualization" type="data_plotter.py" />
 
   <!--  ************** additional nodes ***************  -->