MOD sample plot

kimushun1101 · Feb 3, 2024 · 63d2ba0 · 63d2ba0
1 parent e7cc300
commit 63d2ba0
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 28 deletions.
diff --git a/README.md b/README.md
@@ -36,9 +36,9 @@ Turtlebot3 に制御則を実装するパッケージ．
    colcon build --symlink-install
    ```
 
-## シミュレータと制御則の実行
+## シミュレーターと制御則の実行
 
-1. シミュレータの起動
+1. シミュレーターの起動
    ```
    # Terminal 1
    export LIBGL_ALWAYS_SOFTWARE=1  # オンボードGPU のときはこれをしないとGazebo が暗くなる？
@@ -49,15 +49,15 @@ Turtlebot3 に制御則を実装するパッケージ．
    そのような場合には`Ctrl+C` で一度閉じ，再度
    `ros2 launch turtlebot3_gazebo turtlebot3_dqn_stage1.launch.py`
    を実行する．  
-   それでもロボットモデルが出ない場合には，Gazebo 画面内の左にあるInsert タブから，Turtlebot3(Burger) をクリックしてシミュレータ上にロボットを手動で置く．
+   それでもロボットモデルが出ない場合には，Gazebo 画面内の左にあるInsert タブから，Turtlebot3(Burger) をクリックしてシミュレーター上にロボットを手動で置く．
 2. 新しく別のターミナルを開き，以下のコマンドで制御を開始（コントローラーを実行）
    ```
    # Terminal 2
    source ~/ros2_ws/install/setup.bash
    ros2 run tb3_controller_cpp tb3_controller_node
    ```
 
-シミュレータと制御則を 1 つのターミナルから同時実行させたい場合には，上記の 2 つのターミナルのコマンドを`Ctrl+C` で終了して，以下のコマンドを入力する．
+シミュレーターと制御則を 1 つのターミナルから同時実行させたい場合には，上記の 2 つのターミナルのコマンドを`Ctrl+C` で終了して，以下のコマンドを入力する．
 ```
 # Terminal 1
 source ~/ros2_ws/install/setup.bash
@@ -67,7 +67,7 @@ ros2 launch tb3_controller_cpp simulation_and_controller.launch.yaml
 
 ## パラメーター調整
 
-`# Terminal 1` でシミュレータ，`# Terminal 2` でコントローラーを起動しているものとする．
+`# Terminal 1` でシミュレーター，`# Terminal 2` でコントローラーを起動しているものとする．
 
 1. Gazebo 上のロボットの移動  
    `t` キーを押下して `Translation Mode` に移行してからロボットをドラッグ・アンド・ドロップ．  
@@ -94,20 +94,29 @@ ros2 launch tb3_controller_cpp simulation_and_controller.launch.yaml
    都度 `-p` オプションをつければ，複数のパラメーターを同時に設定することもできる．
    ```
    # Terminal 2
-   ros2 run tb3_controller_cpp tb3_controller_node --ros-args -p Kp:=2.0 -p T:=0.01 -p init_xd:=1.5
+   ros2 run tb3_controller_cpp tb3_controller_node --ros-args -p Kp:=0.5 -p T:=0.01 -p init_xd:=3.0
    ```
+4. Launch ファイルに反映
+   決定したパラメータを [launch/simulation_and_controller.launch.yaml](./launch/simulation_and_controller.launch.yaml#L9) に書き込む．
 
 ## 結果出力
 
+`# Terminal 1` でシミュレーター，`# Terminal 2` でコントローラーを起動しているものとする．
+
 1. データの記録  
    rosbag2 を使用してデータの記録を開始する．
    ```
-   # Terminal 3
+   # Terminal 4
    mkdir -p ~/ros2_ws/src/tb3_controller_cpp/result
    cd ~/ros2_ws/src/tb3_controller_cpp/result
    ros2 bag record /scan /xd /cmd_vel
    ```
-   `# Terminal 2` で記録したい制御則を実行し，`# Terminal 3` を `Ctrl+C` することで記録を終了する．
+   目標値の変更を行う．
+   ```
+   # Terminal 3
+   ros2 topic pub /xd std_msgs/msg/Float32 "data: 3.0"
+   ```
+   記録したい動作が終了した後，`# Terminal 3` を `Ctrl+C` することで記録を終了する．
 2. グラフを書く
    ```
    # Terminal 3
@@ -133,7 +142,7 @@ ros2 launch tb3_controller_cpp simulation_and_controller.launch.yaml
    ros2 run tb3_controller_cpp tb3_controller_node
    ```
 
-シミュレータと制御則を 1 つのターミナルから同時実行させたい場合には，以下のコマンドを入力する．
+シミュレーターと制御則を 1 つのターミナルから同時実行させたい場合には，以下のコマンドを入力する．
 ```
 # SSH raspberry Pi 1
 source ~/ros2_ws/install/setup.bash

diff --git a/result/sample_plot.xml b/result/sample_plot.xml
@@ -1,21 +1,22 @@
 <?xml version='1.0' encoding='UTF-8'?>
 <root>
- <tabbed_widget name="Main Window" parent="main_window">
+ <tabbed_widget parent="main_window" name="Main Window">
   <Tab containers="1" tab_name="tab1">
    <Container>
-    <DockSplitter orientation="-" sizes="0.5;0.5" count="2">
+    <DockSplitter count="2" sizes="0.5;0.5" orientation="-">
      <DockArea name="...">
-      <plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
-       <range left="1.054438" right="12.682923" top="3.061142" bottom="0.953732"/>
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range top="3.081903" right="22.341857" bottom="0.915360" left="0.000021"/>
        <limitY/>
-       <curve name="/scan/ranges[180]" color="#1f77b4"/>
+       <curve color="#1f77b4" name="/scan/ranges[180]"/>
+       <curve color="#d62728" name="/xd/data"/>
       </plot>
      </DockArea>
      <DockArea name="...">
-      <plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
-       <range left="1.054438" right="12.682923" top="1.023134" bottom="-0.030571"/>
+      <plot flip_x="false" style="Lines" flip_y="false" mode="TimeSeries">
+       <range top="2.073990" right="22.341857" bottom="-0.081643" left="0.000021"/>
        <limitY/>
-       <curve name="/cmd_vel/linear/x" color="#d62728"/>
+       <curve color="#1ac938" name="/cmd_vel/linear/x"/>
       </plot>
      </DockArea>
     </DockSplitter>
@@ -28,7 +29,7 @@
  <!-- - - - - - - - - - - - - - - -->
  <Plugins>
   <plugin ID="DataLoad CSV">
-   <default delimiter="0" time_axis=""/>
+   <default time_axis="" delimiter="0"/>
   </plugin>
   <plugin ID="DataLoad MCAP"/>
   <plugin ID="DataLoad ROS2 bags">
@@ -61,7 +62,7 @@
  <!-- - - - - - - - - - - - - - - -->
  <previouslyLoaded_Datafiles>
   <fileInfo prefix="" filename="sample_rosbag/metadata.yaml">
-   <selected_datasources value="/cmd_vel;/scan"/>
+   <selected_datasources value="/cmd_vel;/scan;/xd"/>
    <plugin ID="DataLoad ROS2 bags">
     <use_header_stamp value="false"/>
     <discard_large_arrays value="true"/>

diff --git a/result/sample_rosbag/metadata.yaml b/result/sample_rosbag/metadata.yaml
@@ -2,31 +2,37 @@ rosbag2_bagfile_information:
   version: 5
   storage_identifier: sqlite3
   duration:
-    nanoseconds: 12682922331
+    nanoseconds: 22341857440
   starting_time:
-    nanoseconds_since_epoch: 1706882196644211249
-  message_count: 1357
+    nanoseconds_since_epoch: 1706918394892065765
+  message_count: 22423
   topics_with_message_count:
     - topic_metadata:
         name: /cmd_vel
         type: geometry_msgs/msg/Twist
         serialization_format: cdr
         offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
-      message_count: 1293
+      message_count: 22308
     - topic_metadata:
         name: /scan
         type: sensor_msgs/msg/LaserScan
         serialization_format: cdr
         offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 2\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
-      message_count: 64
+      message_count: 98
+    - topic_metadata:
+        name: /xd
+        type: std_msgs/msg/Float32
+        serialization_format: cdr
+        offered_qos_profiles: "- history: 3\n  depth: 0\n  reliability: 1\n  durability: 1\n  deadline:\n    sec: 9223372036\n    nsec: 854775807\n  lifespan:\n    sec: 9223372036\n    nsec: 854775807\n  liveliness: 1\n  liveliness_lease_duration:\n    sec: 9223372036\n    nsec: 854775807\n  avoid_ros_namespace_conventions: false"
+      message_count: 17
   compression_format: ""
   compression_mode: ""
   relative_file_paths:
     - sample_rosbag_0.db3
   files:
     - path: sample_rosbag_0.db3
       starting_time:
-        nanoseconds_since_epoch: 1706882196644211249
+        nanoseconds_since_epoch: 1706918394892065765
       duration:
-        nanoseconds: 12682922331
-      message_count: 1357
+        nanoseconds: 22341857440
+      message_count: 22423
diff --git a/result/sample_rosbag/sample_rosbag_0.db3 b/result/sample_rosbag/sample_rosbag_0.db3
diff --git a/src/tb3_controller_node.cpp b/src/tb3_controller_node.cpp
@@ -24,7 +24,7 @@ class Tb3Controller : public rclcpp::Node
   {
     this->declare_parameter<std::float_t>("Kp", 1.0);
     this->get_parameter("Kp", Kp_);
-    this->declare_parameter<std::float_t>("T", 0.001);
+    this->declare_parameter<std::float_t>("T", 0.01);
     this->get_parameter("T", T_);
     this->declare_parameter<std::float_t>("init_xd", 1.0);
     this->get_parameter("init_xd", xd_);