update

masakifujiwara1 · Apr 19, 2023 · f6263e0 · f6263e0
1 parent d44639c
commit f6263e0
Show file tree

Hide file tree

Showing 9 changed files with 151 additions and 43 deletions.
diff --git a/experiments/experiment_cit3f_with_gamma.sh b/experiments/experiment_cit3f_with_gamma.sh
@@ -1,5 +1,5 @@
 for i in `seq 1`
 do
-  roslaunch real_tsudanuma2-3_sim nav_cloning_sim.launch script:=nav_cloning_with_direction_node.py mode:=selected_training world_name:=tsudanuma2-3_v2.3.3.world map_file:=real_tsudanuma2-3.yaml waypoints_file:=real_tsudanuma2-3_way.yaml dist_err:=0.8 initial_pose_x:=-5.0 initial_pose_y:=7.7 initial_pose_a:=3.14 use_waypoint_nav:=true robot_x:=0.0 robot_y:=0.0 robot_Y:=0.0
+  roslaunch real_tsudanuma2-3_sim nav_cloning_sim.launch script:=nav_cloning_with_direction_node.py mode:=selected_training world_name:=tsudanuma2-3_v2.3.3.world map_file:=real_tsudanuma2-3_v2.yaml waypoints_file:=real_tsudanuma2-3_way.yaml dist_err:=0.8 initial_pose_x:=-5.0 initial_pose_y:=7.7 initial_pose_a:=3.14 use_waypoint_nav:=true robot_x:=0.0 robot_y:=0.0 robot_Y:=0.0
   sleep 10
 done
diff --git a/experiments/experiment_cit3f_with_turtlebot3.sh b/experiments/experiment_cit3f_with_turtlebot3.sh
@@ -0,0 +1,5 @@
+for i in `seq 1`
+do
+  roslaunch real_tsudanuma2-3_sim nav_cloning_sim_turtlebot3.launch script:=nav_cloning_with_direction_node.py mode:=selected_training world_name:=tsudanuma2-3_v2.3.3.world map_file:=real_tsudanuma2-3.yaml waypoints_file:=real_tsudanuma2-3_way.yaml dist_err:=0.8 initial_pose_x:=-5.0 initial_pose_y:=7.7 initial_pose_a:=3.14 use_waypoint_nav:=true robot_x:=0.0 robot_y:=0.0 robot_Y:=0.0
+  sleep 10
+done
diff --git a/launch/gamma_navigation.launch b/launch/gamma_navigation.launch
@@ -33,18 +33,18 @@
   <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
 
   <!-- AMCL -->
-  <!-- <include file="$(find turtlebot3_navigation)/launch/amcl.launch">
+  <include file="$(find turtlebot3_navigation)/launch/amcl.launch">
     <arg name="initial_pose_x" value="$(arg initial_pose_x)" />
     <arg name="initial_pose_y" value="$(arg initial_pose_y)" />
     <arg name="initial_pose_a" value="$(arg initial_pose_a)" />
-  </include> -->
+  </include>
 
-  <include file="$(find emcl2)/launch/emcl2.launch">
+  <!-- <include file="$(find emcl2)/launch/emcl2.launch">
     <arg name="scan_topic"  default="scan"/>
     <arg name="initial_pose_x" value="$(arg initial_pose_x)" />
     <arg name="initial_pose_y" value="$(arg initial_pose_y)" />
     <arg name="initial_pose_a" value="$(arg initial_pose_a)" />
-  </include>
+  </include> -->
 
   <!-- move_base -->
   <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
@@ -69,7 +69,7 @@
 
   <!-- rviz -->
   <group if="$(arg open_rviz)"> 
-    <node pkg="rviz" type="rviz" name="rviz" required="true"
-          args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
+    <!-- <node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find waypoint_visualization)/rviz_config/nav_manager_v5.rviz"/> -->
+    <node pkg="rviz" type="rviz" name="rviz" required="true" args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
   </group>
 </launch>
diff --git a/launch/nav_cloning_sim.launch b/launch/nav_cloning_sim.launch
@@ -26,7 +26,7 @@
     <arg name="debug" value="false"/>
   </include>
 
-  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find orne_description)/urdf/gamma/orne_gamma.urdf.xacro" />
+  <param name="robot_description" command="$(find xacro)/xacro $(find orne_description)/urdf/gamma/orne_gamma.urdf.xacro" />
 
   <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model mobile_base -x $(arg robot_x) -y $(arg robot_y) -z 0.28 -Y $(arg robot_Y) -param robot_description" />
 

diff --git a/launch/nav_cloning_sim_turtlebot3.launch b/launch/nav_cloning_sim_turtlebot3.launch
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="script" default="nav_cloning_node.py"/>
+  <arg name="mode" default="use_dl_output"/>
+  <arg name="world_name" default="willow_garage.world"/>
+  <arg name="map_file" default="willowgarage.yaml"/>
+  <arg name="waypoints_file" default="willow_loop.yaml"/>
+  <arg name="dist_err" default="0.8"/>
+  <arg name="initial_pose_x" default="-10.78" />
+  <arg name="initial_pose_y" default="-16.78" />
+  <arg name="initial_pose_a" default="0.0" />
+  <arg name="use_waypoint_nav" default="false" />
+  <arg name="use_initpose" default="false" />
+  <arg name="use_update_amcl_param" default="false" />
+  <arg name="robot_x" default="0.0" />
+  <arg name="robot_y" default="0.0" />
+  <arg name="robot_Y" default="0.0" />
+  <rosparam param="/nav_cloning_node/mode" subst_value="True">$(arg mode)</rosparam>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find real_tsudanuma2-3_sim)/world/$(arg world_name)"/> 
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="false"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+  </include>
+
+  <param name="robot_description" command="$(find xacro)/xacro $(find turtlebot3_description)/urdf/turtlebot3_waffle_pi.urdf.xacro" />
+
+  <node pkg="gazebo_ros" type="spawn_model" name="spawn_urdf" args="-urdf -model mobile_base -x $(arg robot_x) -y $(arg robot_y) -z 0.28 -Y $(arg robot_Y) -param robot_description" />
+
+  <group if="$(arg use_initpose)">
+    <node pkg="nav_cloning" type="initpose" name="initpose"/>
+  </group>
+
+  <!-- <node pkg="timed_roslaunch" type="timed_roslaunch.sh"
+        args="5 nav_cloning nav_cloning.launch script:=$(arg script)"
+        name="timed_roslaunch" output="screen" /> -->
+
+  <node pkg="timed_roslaunch" type="timed_roslaunch.sh"
+        args="8 real_tsudanuma2-3_sim turtlebot3_navigation.launch model:=waffle_pi map_file:=$(find real_tsudanuma2-3_sim)/maps/$(arg map_file) waypoints_file:=$(find real_tsudanuma2-3_sim)/maps/$(arg waypoints_file) dist_err:=$(arg dist_err) initial_pose_x:=$(arg initial_pose_x) initial_pose_y:=$(arg initial_pose_y) initial_pose_a:=$(arg initial_pose_a) use_waypoint_nav:=$(arg use_waypoint_nav) use_update_amcl_param:=$(arg use_update_amcl_param)"
+        name="timed_roslaunch2" >
+  </node>
+
+  <node pkg="timed_roslaunch" type="timed_roslaunch.sh"
+        args="15 nav_cloning start_wp_nav.launch"
+        name="timed_roslaunch3" >
+  </node>
+
+  <node pkg="timed_roslaunch" type="timed_roslaunch.sh"
+        args="15 nav_cloning piechart.launch"
+        name="timed_roslaunch4" >
+  </node>
+</launch>
diff --git a/launch/turtlebot3_navigation.launch b/launch/turtlebot3_navigation.launch
@@ -40,22 +40,28 @@
       <arg name="update_min_a" value="0.05" />
     </include>
   </group> -->
-  <include file="$(find turtlebot3_navigation)/launch/amcl.launch">
+  <!-- <include file="$(find turtlebot3_navigation)/launch/amcl.launch">
     <arg name="initial_pose_x" value="$(arg initial_pose_x)" />
     <arg name="initial_pose_y" value="$(arg initial_pose_y)" />
     <arg name="initial_pose_a" value="$(arg initial_pose_a)" />
     <arg name="update_min_d" value="0.05" if="$(arg use_update_amcl_param)" />
     <arg name="update_min_a" value="0.05" if="$(arg use_update_amcl_param)" />
+  </include> -->
+  <include file="$(find emcl2)/launch/emcl2.launch">
+    <arg name="scan_topic"  default="scan"/>
+    <arg name="initial_pose_x" value="$(arg initial_pose_x)" />
+    <arg name="initial_pose_y" value="$(arg initial_pose_y)" />
+    <arg name="initial_pose_a" value="$(arg initial_pose_a)" />
   </include>
 
   <!-- move_base -->
   <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
     <arg name="model" value="$(arg model)" />
     <arg name="move_forward_only" value="$(arg move_forward_only)"/>
-    <arg name="cmd_vel_topic" value="/nav_vel"/>
+    <arg name="cmd_vel_topic" value="/cmd_vel"/>
   </include>
 
-  <group unless="$(arg use_waypoint_nav)">
+  <!-- <group unless="$(arg use_waypoint_nav)">
     <node name="waypoints_nav" pkg="fulanghua_waypoints_nav" type="waypoints_nav" output="screen">
       <param name="filename" value="$(arg waypoints_file)"/>
       <param name="dist_err" value="$(arg dist_err)"/>
@@ -67,7 +73,7 @@
       <param name="dist_err" value="$(arg dist_err)"/>
       <param name="max_update_rate" value="$(arg rate)"/>
       <param name="loop_flag" value="$(arg loop)"/>    </node>
-  </group>
+  </group> -->
 
   <!-- rviz -->
   <group if="$(arg open_rviz)"> 

diff --git a/maps/real_tsudanuma2-3_v2.pgm b/maps/real_tsudanuma2-3_v2.pgm
diff --git a/maps/real_tsudanuma2-3_v2.yaml b/maps/real_tsudanuma2-3_v2.yaml
@@ -0,0 +1,7 @@
+image: real_tsudanuma2-3_v2.pgm
+resolution: 0.050000
+origin: [-100.000000, -100.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
diff --git a/maps/real_tsudanuma2-3_way.yaml b/maps/real_tsudanuma2-3_way.yaml
@@ -40,19 +40,19 @@ waypoints:
         z: 0.0033913
         function: run
     - point:
-        x: -1.8463
-        y: 29.8534
-        z: 0.00338987
+        x: -2.10108
+        y: 29.2147
+        z: 0.00339082
         function: run_left
     - point:
-        x: -7.75251
-        y: 29.8424
+        x: -7.96293
+        y: 29.168
         z: 0.00339273
         function: run
     - point:
-        x: -9.81348
-        y: 28.5559
-        z: 0.00339463
+        x: -9.99054
+        y: 28.0651
+        z: 0.00339415
         function: run_left
     - point:
         x: -9.94171
@@ -105,18 +105,18 @@ waypoints:
         z: 0.00395485
         function: run_go
     - point:
-        x: -9.76388
-        y: 28.4995
-        z: 0.00338892
+        x: -10.0127
+        y: 28.1364
+        z: 0.00338987
         function: run
     - point:
-        x: -7.72706
-        y: 29.8374
-        z: 0.00337987
+        x: -8.03327
+        y: 29.1022
+        z: 0.00338082
         function: run_right
     - point:
-        x: -1.9166
-        y: 29.8458
+        x: -2.11331
+        y: 29.2315
         z: 0.0033999
         function: run
     - point:
@@ -145,18 +145,18 @@ waypoints:
         z: 0.00395485
         function: run_right
     - point:
-        x: -9.74743
-        y: 28.5696
-        z: 0.00338892
+        x: -9.97009
+        y: 28.2084
+        z: 0.00338844
         function: run
     - point:
-        x: -7.71707
-        y: 29.8689
-        z: 0.00337987
+        x: -8.02122
+        y: 29.1431
+        z: 0.00338082
         function: run_right
     - point:
-        x: -2.0291
-        y: 29.88
+        x: -2.05759
+        y: 29.2247
         z: 0.00339942
         function: run
     - point:
@@ -220,18 +220,18 @@ waypoints:
         z: 0.00340228
         function: run
     - point:
-        x: -1.96192
-        y: 29.8389
-        z: 0.00339727
+        x: -2.11918
+        y: 29.2901
+        z: 0.00339822
         function: run_left
     - point:
-        x: -7.75182
-        y: 29.7992
+        x: -8.06326
+        y: 29.0992
         z: 0.00338987
         function: run
     - point:
-        x: -9.84999
-        y: 28.5019
+        x: -9.99877
+        y: 28.0794
         z: 0.0033894
         function: run_left
     - point: