From: grpetry <gabriel.petry26@gmail.com>
Date: Thu, 14 Nov 2019 17:34:52 +0000 (-0300)
Subject: Added navigation config files
X-Git-Tag: petryTCC~3
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=1970f60edf89a0f9ece8a6fbd83c975f7fda22ba;p=twil.git

Added navigation config files
---

diff --git a/twil_2dnav/config/base_local_planner_params.yaml b/twil_2dnav/config/base_local_planner_params.yaml
new file mode 100644
index 0000000..87dbd70
--- /dev/null
+++ b/twil_2dnav/config/base_local_planner_params.yaml
@@ -0,0 +1,11 @@
+TrajectoryPlannerROS:
+  max_vel_x: 1 #(vel angular max = 2*pi)
+  min_vel_x: 0.1 #zona morta - olhar dissertação thiago
+  max_vel_theta: 1.0 #2*(max_vel_x) / wheelbase
+  min_in_place_vel_theta: 0.4
+
+  acc_lim_theta: 3.2
+  acc_lim_x: 2.5
+  acc_lim_y: 2.5
+
+  holonomic_robot: false
diff --git a/twil_2dnav/config/costmap_common_params.yaml b/twil_2dnav/config/costmap_common_params.yaml
index 539e8bf..acb62f4 100644
--- a/twil_2dnav/config/costmap_common_params.yaml
+++ b/twil_2dnav/config/costmap_common_params.yaml
@@ -1,11 +1,9 @@
 obstacle_range: 2.5
 raytrace_range: 3.0
-footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
-#robot_radius: ir_of_robot
-inflation_radius: 0.55
+#footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
+robot_radius: 0.3 #wheelbase/2
+inflation_radius: 0.3
 
-observation_sources: laser_scan_sensor point_cloud_sensor
+observation_sources: point_cloud_sensor
 
-laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}
-
-point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}
+point_cloud_sensor: {sensor_frame: camera_link, data_type: PointCloud2, topic: /camera/depth/points, marking: true, clearing: true}
diff --git a/twil_2dnav/config/global_costmap_params.yaml b/twil_2dnav/config/global_costmap_params.yaml
new file mode 100644
index 0000000..43a268f
--- /dev/null
+++ b/twil_2dnav/config/global_costmap_params.yaml
@@ -0,0 +1,5 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: twil_origin
+  update_frequency: 1.0
+  static_map: true
diff --git a/twil_2dnav/config/local_costmap_params.yaml b/twil_2dnav/config/local_costmap_params.yaml
new file mode 100644
index 0000000..0364d11
--- /dev/null
+++ b/twil_2dnav/config/local_costmap_params.yaml
@@ -0,0 +1,10 @@
+local_costmap:
+  global_frame: odom
+  robot_base_frame: twil_origin
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: true
+  rolling_window: true
+  width: 6.0
+  height: 6.0
+  resolution: 0.01
diff --git a/twil_2dnav/launch/move_base.launch b/twil_2dnav/launch/move_base.launch
new file mode 100644
index 0000000..a52c9b7
--- /dev/null
+++ b/twil_2dnav/launch/move_base.launch
@@ -0,0 +1,22 @@
+<launch>
+
+   <master auto="start"/>
+ <!-- Run the map server --> 
+    <node name="map_server" pkg="map_server" type="map_server" args="$(find twil_2dnav)/map/map.yaml"/>
+
+ <!--- Run AMCL --> 
+    <!--include file="$(find amcl)/examples/amcl_omni.launch" /-->
+
+   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+	    <rosparam file="$(find twil_2dnav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" /> 
+	    <rosparam file="$(find twil_2dnav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+	    <rosparam file="$(find twil_2dnav)/config/local_costmap_params.yaml" command="load" />
+	    <rosparam file="$(find twil_2dnav)/config/global_costmap_params.yaml" command="load" /> 
+	    <rosparam file="$(find twil_2dnav)/config/base_local_planner_params.yaml" command="load" />
+
+	   
+ </node>
+
+
+
+</launch> 
diff --git a/twil_2dnav/map/map.yaml b/twil_2dnav/map/map.yaml
new file mode 100644
index 0000000..b9d2a71
--- /dev/null
+++ b/twil_2dnav/map/map.yaml
@@ -0,0 +1,6 @@
+image: planta_cores.pgm
+resolution: 0.01
+origin: [-16.0, -2.0, 0.0]
+occupied_thresh: 0.7
+free_thresh: 0.3
+negate: 0
diff --git a/twil_2dnav/map/planta_cores.pgm b/twil_2dnav/map/planta_cores.pgm
new file mode 100644
index 0000000..a19aadd
Binary files /dev/null and b/twil_2dnav/map/planta_cores.pgm differ
diff --git a/twil_bringup/launch/gazebo_rviz.launch b/twil_bringup/launch/gazebo_rviz.launch
index c254b5e..a511f06 100644
--- a/twil_bringup/launch/gazebo_rviz.launch
+++ b/twil_bringup/launch/gazebo_rviz.launch
@@ -1,5 +1,5 @@
 <launch>
-	<arg name="paused" default="true"/>
+	<arg name="paused" default="false"/>
 	<arg name="headless" default="false"/>
 	<arg name="use_sim_time" default="true"/>
 	<arg name="wam" default="false"/>
@@ -7,29 +7,28 @@
 	<arg name="controller" default="joint_effort_controller"/>
         <arg name="config" default="$(find twil_bringup)/config/$(arg controller).yaml"/>
 
+	<remap from="/twist_mrac_linearizing_controller/odom" to="/odom" />
+	<remap from="/twist_mrac_linearizing_controller/command" to="/cmd_vel" />
+
 	<include file="$(find twil_description)/launch/gazebo.launch" >
 		<arg name="paused" value="$(arg paused)"/>
 		<arg name="headless" value="$(arg headless)"/>
 		<arg name="use_sim_time" value="$(arg use_sim_time)"/>
 		<arg name="wam" value="$(arg wam)"/>
+		
 	</include>
 
+
 	<include file="$(find twil_bringup)/launch/$(arg controller).launch" />
+	
 
- 	<!--node name="pose2d_stamp" pkg="pose2d_trajectories" type="pose2d_stamp" respawn="false" output="screen" args="_frame_id:=map" /-->
 
   	<node pkg="tf2_ros" type="static_transform_publisher" name="odom_frame_publisher" args="0 0 0 0 0 0 1 map odom" />
 
-	<!--node pkg="tf2_ros" type="static_transform_publisher" name="twil_origin_publisher" args="0 0 0 0 0 0 1 map twil_origin" /-->
-
 	<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
 
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find twil_bringup)/rviz/gazebo_rviz.rviz" required="true"/>
 
 	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
 
- 	 <!--rosparam file="$(find twil_bringup)/config/odometry_publisher.yaml" command="load"/-->
-
- 	 <!--node pkg="arc_odometry" type="odometry_publisher" name="odom_publisher" /-->
-
 </launch>
diff --git a/twil_bringup/rviz/gazebo_rviz.rviz b/twil_bringup/rviz/gazebo_rviz.rviz
index 3c490e1..07df110 100644
--- a/twil_bringup/rviz/gazebo_rviz.rviz
+++ b/twil_bringup/rviz/gazebo_rviz.rviz
@@ -7,9 +7,9 @@ Panels:
         - /Global Options1
         - /Status1
         - /Odometry1
-        - /Pose1
+        - /Map1
       Splitter Ratio: 0.5
-    Tree Height: 549
+    Tree Height: 728
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -278,20 +278,15 @@ Visualization Manager:
       Topic: /twist_mrac_linearizing_controller/odom
       Unreliable: false
       Value: true
-    - Alpha: 1
-      Axes Length: 1
-      Axes Radius: 0.10000000149011612
-      Class: rviz/Pose
-      Color: 85; 0; 255
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
       Enabled: true
-      Head Length: 0.30000001192092896
-      Head Radius: 0.10000000149011612
-      Name: Pose
-      Shaft Length: 1
-      Shaft Radius: 0.05000000074505806
-      Shape: Arrow
-      Topic: /command_stamped
+      Name: Map
+      Topic: /map
       Unreliable: false
+      Use Timestamp: false
       Value: true
   Enabled: true
   Global Options:
@@ -321,33 +316,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.673752784729004
+      Distance: 8.001239776611328
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.26802945137023926
-        Y: -0.270676851272583
-        Z: 0.5226089954376221
+        X: -0.16269730031490326
+        Y: -0.3901738226413727
+        Z: 1.328372836112976
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5353978276252747
+      Pitch: 0.8203981518745422
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.540398120880127
+      Yaw: 4.728579998016357
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 1025
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000031b00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d0000031b00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -356,6 +351,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1200
-  X: 171
-  Y: 127
+  Width: 1853
+  X: 67
+  Y: 27