Fixed empty.yaml file, fixed gts (#104)

* Fixed empty.yaml file, fixed gts Corrected origin, fixed gts to refer to the global map frame * Added amcl initial pose on empty world map. * Reverted unnecessary change * Added conditional to initial amcl position * Added NAVIGATION env variable * Changed NAVIGATION to LOCALIZATION * Changed logic to improve scalability
RoboticaUtnFrba · Oct 20, 2019 · 3c5e68f · 3c5e68f
1 parent 502f064
commit 3c5e68f
Show file tree

Hide file tree

Showing 6 changed files with 23 additions and 39 deletions.
diff --git a/ca_description/urdf/create_base_gazebo.xacro b/ca_description/urdf/create_base_gazebo.xacro
@@ -26,7 +26,7 @@
         <publishOdomTF>true</publishOdomTF>
         <publishWheelJointState>true</publishWheelJointState>
         <legacyMode>false</legacyMode>
-        <odometrySource>world</odometrySource>
+        <odometrySource>map</odometrySource>
         <publishTf>1</publishTf>
       </plugin>
 
@@ -37,7 +37,7 @@
         <bodyName>base_link</bodyName>
         <topicName>gts</topicName>
         <gaussianNoise>0</gaussianNoise>
-        <frameName>map</frameName>
+        <frameName>/map</frameName>
         <xyzOffsets>0 0 0</xyzOffsets>
         <rpyOffsets>0 0 0</rpyOffsets>
       </plugin>

diff --git a/ca_gazebo/launch/create_empty_world.launch b/ca_gazebo/launch/create_empty_world.launch
@@ -14,6 +14,16 @@
 		<arg name="verbose" value="true"/>
 		<arg name="world_name" value="$(find ca_gazebo)/worlds/$(arg env).world"/>
 	</include>
+
+	<group if="$(eval str(arg('env'))=='empty')">
+			<param name="create1/amcl/initial_pose_x" value="-3"/>
+			<param name="create1/amcl/initial_pose_y" value="4"/>
+			<param name="create1/amcl/initial_pose_a" value="0"/>
+			<param name="create2/amcl/initial_pose_x" value="5"/>
+			<param name="create2/amcl/initial_pose_y" value="1"/>
+			<param name="create2/amcl/initial_pose_a" value="1.5708"/>
+	</group>
+
 
 	<!-- Spawn robot/s -->
 	<include file="$(find ca_gazebo)/launch/spawn_multirobot.launch"/>

diff --git a/ca_gazebo/launch/spawn_multirobot.launch b/ca_gazebo/launch/spawn_multirobot.launch
@@ -2,6 +2,7 @@
   <!-- https://answers.ros.org/question/229489/how-do-i-create-dynamic-launch-files/ -->
   <arg name="nr" default="$(optenv NUM_ROBOTS 1)"/>
   <arg name="rviz" default="$(optenv RVIZ true)"/>
+  <arg name="localization" default="$(optenv LOCALIZATION none)"/>
 
   <!-- Start 1 robot -->
   <group ns="$(eval 'create' + str(arg('nr')))">
@@ -11,9 +12,12 @@
 
     <param name="tf_prefix" value="create$(arg nr)_tf" />
 
-    <!-- Navigation -->
-    <include file="$(find ca_move_base)/launch/navigate.launch">
+    <!-- localization -->
+
+    <include if="$(eval str(arg('localization'))!='none')"
+     file="$(find ca_move_base)/launch/navigate.launch">
       <arg name="robot_name" value="create$(arg nr)"/>
+      <arg name="localization" value="$(arg localization)"/>
     </include>
 
     <!-- RViz -->

diff --git a/ca_gazebo/worlds/empty.world b/ca_gazebo/worlds/empty.world
@@ -1,42 +1,13 @@
-<?xml version="1.0"?>
+<?xml version="1.0" ?>
 <sdf version="1.6">
   <world name="default">
     <!-- A global light source -->
     <include>
       <uri>model://sun</uri>
     </include>
-
     <!-- A ground plane -->
     <include>
       <uri>model://ground_plane</uri>
     </include>
-
-    <physics type="ode">
-      <real_time_update_rate>1000.0</real_time_update_rate>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <iters>150</iters>
-          <precon_iters>0</precon_iters>
-          <sor>1.400000</sor>
-          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
-        </solver>
-        <constraints>
-          <cfm>0.00001</cfm>
-          <erp>0.2</erp>
-          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
-          <contact_surface_layer>0.01000</contact_surface_layer>
-        </constraints>
-      </ode>
-    </physics>
   </world>
-
-  <scene>
-    <ambient>0.4 0.4 0.4 1</ambient>
-    <background>0.7 0.7 0.7 1</background>
-    <shadows>true</shadows>
-  </scene>
-
 </sdf>
diff --git a/navigation/ca_move_base/launch/navigate.launch b/navigation/ca_move_base/launch/navigate.launch
@@ -1,7 +1,6 @@
 <launch>
   <arg name="robot_name"/>
-  <arg name="slam" default="$(optenv SLAM false)"/>
-
+  <arg name="localization"/>
 	<!-- Bumper to point cloud -->
 	<include file="$(find ca_bumper2pc)/launch/standalone.launch"/>
 
@@ -11,12 +10,12 @@
   </include>
 
 	<!-- AMCL -->
-  <include if="$(eval arg('slam') == False)" file="$(find ca_move_base)/launch/amcl.launch">
+  <include if="$(eval str(arg('localization')) == 'amcl')" file="$(find ca_move_base)/launch/amcl.launch">
     <arg name="ns" value="$(arg robot_name)"/>
   </include>
 
   <!-- slam_gmapping -->
-  <include if="$(eval arg('slam') == True)" file="$(find ca_slam)/launch/slam_gmapping.launch">
+  <include if="$(eval str(arg('localization')) == 'slam')" file="$(find ca_slam)/launch/slam_gmapping.launch">
     <arg name="ns" value="$(arg robot_name)"/>
   </include>
 

diff --git a/navigation/ca_move_base/maps/empty.yaml b/navigation/ca_move_base/maps/empty.yaml
@@ -2,5 +2,5 @@ free_thresh: 0.196
 image: empty.pgm
 negate: 0
 occupied_thresh: 0.65
-origin: [-6.8999999999999915, -5.8999999999999915, 0.0]
+origin: [-12.5, -12.5, 0.0]
 resolution: 0.05