From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Wed, 20 Apr 2022 20:36:26 +0000 (-0300)
Subject: Add GPS.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=a2974be1dbaa982d2b126c33d1ee0a4326c4b930;p=twil.git

Add GPS.
---

diff --git a/twil_bringup/config/dynamics_linearizing_controller.yaml b/twil_bringup/config/dynamics_linearizing_controller.yaml
index 75c3647..45776c7 100644
--- a/twil_bringup/config/dynamics_linearizing_controller.yaml
+++ b/twil_bringup/config/dynamics_linearizing_controller.yaml
@@ -7,6 +7,7 @@
       - caster_base_joint
       - front_caster_wheel_joint
       - front_caster_base_joint
+      - tower_joint
 
 /dynamics_linearizing_controller:
   ros__parameters:
diff --git a/twil_bringup/config/joint_effort_controller.yaml b/twil_bringup/config/joint_effort_controller.yaml
index 1fed36f..5674062 100644
--- a/twil_bringup/config/joint_effort_controller.yaml
+++ b/twil_bringup/config/joint_effort_controller.yaml
@@ -5,6 +5,7 @@
                         - caster_base_joint
                         - front_caster_wheel_joint
                         - front_caster_base_joint
+                        - tower_joint
 
 /left_wheel_joint_effort_controller:
         ros__parameters:
diff --git a/twil_bringup/config/joint_velocity_controller.yaml b/twil_bringup/config/joint_velocity_controller.yaml
index d5bc594..721414c 100644
--- a/twil_bringup/config/joint_velocity_controller.yaml
+++ b/twil_bringup/config/joint_velocity_controller.yaml
@@ -5,6 +5,7 @@
                         - caster_base_joint
                         - front_caster_wheel_joint
                         - front_caster_base_joint
+                        - tower_joint
 
 /left_wheel_joint_velocity_controller:
         ros__parameters:
diff --git a/twil_bringup/config/nonsmooth_backstep_controller.yaml b/twil_bringup/config/nonsmooth_backstep_controller.yaml
index 76ce6bc..ed92ab9 100644
--- a/twil_bringup/config/nonsmooth_backstep_controller.yaml
+++ b/twil_bringup/config/nonsmooth_backstep_controller.yaml
@@ -7,6 +7,7 @@
       - caster_base_joint
       - front_caster_wheel_joint
       - front_caster_base_joint
+      - tower_joint
 
 /nonsmooth_backstep_controller:
   ros__parameters:
diff --git a/twil_bringup/config/nonsmooth_backstep_controller_step.yaml b/twil_bringup/config/nonsmooth_backstep_controller_step.yaml
index 5ec5bc3..66652e9 100644
--- a/twil_bringup/config/nonsmooth_backstep_controller_step.yaml
+++ b/twil_bringup/config/nonsmooth_backstep_controller_step.yaml
@@ -7,6 +7,7 @@
       - caster_base_joint
       - front_caster_wheel_joint
       - front_caster_base_joint
+      - tower_joint
 
 /nonsmooth_backstep_controller:
   ros__parameters:
diff --git a/twil_bringup/config/twist_mrac_linearizing_controller.yaml b/twil_bringup/config/twist_mrac_linearizing_controller.yaml
index 01b3307..aed26a8 100644
--- a/twil_bringup/config/twist_mrac_linearizing_controller.yaml
+++ b/twil_bringup/config/twist_mrac_linearizing_controller.yaml
@@ -7,6 +7,7 @@
       - caster_base_joint
       - front_caster_wheel_joint
       - front_caster_base_joint
+      - tower_joint
 
 /twist_mrac_linearizing_controller:
   ros__parameters:
diff --git a/twil_bringup/launch/gazebo.launch.xml b/twil_bringup/launch/gazebo.launch.xml
index d51f5f8..2e5d498 100644
--- a/twil_bringup/launch/gazebo.launch.xml
+++ b/twil_bringup/launch/gazebo.launch.xml
@@ -24,6 +24,8 @@
 	<arg name="gui" default="true"/>
 	<arg name="use_sim_time" default="true"/>
 	<arg name="wam" default="false"/>
+	<arg name="gps" default="false"/>
+	<arg name="world" default ="worlds/empty_sky.world"/>
 
 	<arg name="controller" default="joint_effort_controller"/>
 	<arg name="config" default="$(find-pkg-share twil_bringup)/config/$(var controller).yaml"/>
@@ -33,6 +35,8 @@
 		<arg name="gui" value="$(var gui)"/>
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
 		<arg name="wam" value="$(var wam)"/>
+		<arg name="gps" value="$(var gps)"/>
+		<arg name="world" value="$(var world)"/>
 	</include>
 
 	<include file="$(find-pkg-share twil_bringup)/launch/$(var controller).launch.xml" >
diff --git a/twil_bringup/launch/gazebo8.launch.xml b/twil_bringup/launch/gazebo8.launch.xml
index eaabb9a..1d2cbe4 100644
--- a/twil_bringup/launch/gazebo8.launch.xml
+++ b/twil_bringup/launch/gazebo8.launch.xml
@@ -20,35 +20,45 @@
 *******************************************************************************-->
 
 <launch>
-  <arg name="pause" default="true"/>
-  <arg name="gui" default="true"/>
-  <arg name="use_sim_time" default="true"/>
-  <arg name="wam" default="false"/>
+	<arg name="pause" default="true"/>
+	<arg name="gui" default="true"/>
+	<arg name="use_sim_time" default="true"/>
+	<arg name="wam" default="false"/>
+	<arg name="gps" default="false"/>
+	<arg name="world" default ="worlds/empty_sky.world"/>
 
-  <arg name="controller" default="nonsmooth_backstep_controller"/>
-  <arg name="config" default="$(find-pkg-share twil_bringup)/config/$(var controller).yaml"/>
+	<arg name="controller" default="nonsmooth_backstep_controller"/>
+	<arg name="config" default="$(find-pkg-share twil_bringup)/config/$(var controller).yaml"/>
 
-  <include file="$(find-pkgshare twil_bringup)/launch/gazebo.launch.xml">
-    <arg name="pause" value="$(var pause)"/>
-    <arg name="gui" value="$(var gui)"/>
-    <arg name="use_sim_time" value="$(var use_sim_time)"/>
-    <arg name="wam" value="$(var wam)"/>
-    <arg name="controller" value="$(var controller)"/>
-    <arg name="config" value="$(var config)"/>
-  </include>
+	<include file="$(find-pkg-share twil_bringup)/launch/gazebo.launch.xml">
+		<arg name="pause" value="$(var pause)"/>
+		<arg name="gui" value="$(var gui)"/>
+		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+		<arg name="wam" value="$(var wam)"/>
+		<arg name="gps" value="$(var gps)"/>
+		<arg name="world" value="$(var world)"/>
+		<arg name="controller" value="$(var controller)"/>
+		<arg name="config" value="$(var config)"/>
+	</include>
 
-  <remap from="/command" to="/$(arg controller)/command" />
+	<node name="eight_trajectory" pkg="pose2d_trajectories" exec="eight_trajectory">
+		<param name="x" value="0.0"/>
+		<param name="y" value="-0.5"/>
+		<param name="radius" value="1.0"/>
+		<param name="ang_vel" value="0.1"/>
+		<param name="use_sim_time" value="$(var use_sim_time)"/>
+		<remap from="command" to="/$(var controller)/command" />
+	</node>
 
-  <node name="eight_trajectory" pkg="pose2d_trajectories" exec="eight_trajectory" respawn="false" output="screen" args="_x:=0.0 _y:=-0.5  _radius:=1.0 _ang_vel:=0.1"/>
+	<node name="pose2d_stamp" pkg="pose2d_trajectories" exec="pose2d_stamp">
+		<param name="frame_id" value="map"/>
+		<param name="use_sim_time" value="$(var use_sim_time)"/>
+		<remap from="command" to="/$(var controller)/command" />
+	</node>
 
-  <node name="pose2d_stamp" pkg="pose2d_trajectories" exec="pose2d_stamp" respawn="false" output="screen" args="_frame_id:=map" />
-
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="odom_frame_publisher" args="0 0 0 0 0 0 1 map odom" />
-
-  <include file="$(find-pkg-share twil_description)/launch/display.launch.xml" />
-
-  <rosparam file="$(find-pkg-share twil_bringup)/config/odometry_publisher.yaml" command="load"/>
-
-  <node pkg="arc_odometry" exec="odometry_publisher" name="odom_publisher" />
+	<node pkg="tf2_ros" exec="static_transform_publisher" name="odom_frame_publisher" args="0 0 0 0 0 0 1 map odom" />
 
+	<include file="$(find-pkg-share twil_description)/launch/display.launch.xml">
+		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+	</include>
 </launch>
diff --git a/twil_bringup/launch/joint_states_odometry.launch.xml b/twil_bringup/launch/joint_states_odometry.launch.xml
new file mode 100644
index 0000000..c5bc47a
--- /dev/null
+++ b/twil_bringup/launch/joint_states_odometry.launch.xml
@@ -0,0 +1,31 @@
+<!--******************************************************************************
+	                       Twil Bringup
+	              Joint States Odometry Launch File
+          Copyright (C) 2022 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        Geneal Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+        
+*******************************************************************************-->
+
+<launch>
+	<arg name="use_sim_time" default="false"/>
+	<arg name="config" default="$(find-pkg-share twil_bringup)/config/odometry_publisher.yaml"/>
+
+	<node pkg="arc_odometry" exec="odometry_publisher" name="odometry_publisher">
+		<param from="$(var config)"/>
+		<param name="use_sim_time" value="$(var use_sim_time)"/>
+	</node>
+
+</launch>
diff --git a/twil_description/launch/display.launch.xml b/twil_description/launch/display.launch.xml
index 36e856a..18e59ae 100644
--- a/twil_description/launch/display.launch.xml
+++ b/twil_description/launch/display.launch.xml
@@ -23,6 +23,7 @@
 	<arg name="wam" default="false"/>
 	<arg name="use_gui" default="false"/>
 	<arg name="d435" default="false"/>
+	<arg name="use_sim_time" default="false"/>
 
 	<node pkg="tf2_ros" exec="static_transform_publisher" name="twil_origin_publisher" args="0 0 0 0 0 0 1 map twil_origin" />
 	<node if="$(var use_gui)" name="joint_state_publisher" pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui" />
@@ -30,5 +31,7 @@
 		<arg name="wam" value="$(var wam)" />
 		<arg name="d435" value="$(var d435)" />
 	</include>
-	<node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share twil_description)/rviz/display.rviz" />
+	<node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share twil_description)/rviz/display.rviz">
+		<param name="use_sim_time" value='$(var use_sim_time)'/>
+	</node>
 </launch>
diff --git a/twil_description/launch/gazebo.launch.xml b/twil_description/launch/gazebo.launch.xml
index ea607c6..1f53b26 100644
--- a/twil_description/launch/gazebo.launch.xml
+++ b/twil_description/launch/gazebo.launch.xml
@@ -25,21 +25,23 @@
 	<arg name="use_sim_time" default="true"/>
 	<arg name="wam" default="false"/>
 	<arg name="d435" default="false"/>
+	<arg name="gps" default="false"/>
 	<arg name="world" default ="worlds/empty_sky.world"/>
 
 	<include file="$(find-pkg-share gazebo_ros)/launch/gazebo.launch.py">
 		<arg name="pause" value="$(var pause)"/>
 		<arg name="gui" value="$(var gui)"/>
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
-		<arg name="world" value="$(var world)" />
+		<arg name="world" value="$(var world)"/>
 		<arg name="extra_gazebo_args" value="--ros-args --params-file $(find-pkg-share twil_description)/config/gazebo.yaml"/>
 	</include>
 
 	<include file="$(find-pkg-share twil_description)/launch/twil.launch.xml" >
 		<arg name="use_sim_time" value="$(var use_sim_time)"/>
-		<arg name="wam" value="$(var wam)" />
-		<arg name="d435" value="$(var d435)" />
+		<arg name="wam" value="$(var wam)"/>
+		<arg name="d435" value="$(var d435)"/>
+		<arg name="gps" value="$(var gps)"/>
 	</include>
 
-	<node name="twil_spawner" pkg="gazebo_ros" exec="spawn_entity.py" args="-topic robot_description -entity twil" />
+	<node name="twil_spawner" pkg="gazebo_ros" exec="spawn_entity.py" args="-topic robot_description -entity twil"/>
 </launch>
diff --git a/twil_description/launch/twil.launch.xml b/twil_description/launch/twil.launch.xml
index db78c72..8b56e4b 100644
--- a/twil_description/launch/twil.launch.xml
+++ b/twil_description/launch/twil.launch.xml
@@ -23,9 +23,10 @@
 	<arg name="use_sim_time" default="false"/>
 	<arg name="wam" default="false"/>
 	<arg name="d435" default="false"/>
+	<arg name="gps" default="false"/>
 
 	<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
-		<param name="robot_description" value="$(command 'xacro $(find-pkg-share twil_description)/xacro/twil.urdf.xacro wam:=$(var wam) d435:=$(var d435) add_plug:=true')" type="str"/>
+		<param name="robot_description" value="$(command 'xacro $(find-pkg-share twil_description)/xacro/twil.urdf.xacro wam:=$(var wam) d435:=$(var d435) add_plug:=true gps:=$(var gps)')" type="str"/>
 		<param name="use_sim_time" value="$(var use_sim_time)"/>
 	</node>
 </launch>
diff --git a/twil_description/rviz/display.rviz b/twil_description/rviz/display.rviz
index 77e012f..c9a6008 100644
--- a/twil_description/rviz/display.rviz
+++ b/twil_description/rviz/display.rviz
@@ -4,8 +4,7 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Global Options1
-        - /Status1
+        - /Ground Truth1/Topic1
       Splitter Ratio: 0.6764705777168274
     Tree Height: 549
   - Class: rviz_common/Selection
@@ -244,6 +243,8 @@ Visualization Manager:
           Value: false
         motor_driver:
           Value: false
+        odom:
+          Value: true
         power_supply:
           Value: false
         right_wheel:
@@ -271,6 +272,8 @@ Visualization Manager:
       Show Names: false
       Tree:
         map:
+          odom:
+            {}
           twil_origin:
             chassis:
               battery:
@@ -314,6 +317,104 @@ Visualization Manager:
                 {}
       Update Interval: 0
       Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/Pose
+      Color: 0; 0; 255
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose Reference
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /command_stamped
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: false
+      Enabled: true
+      Keep: 100
+      Name: Ground Truth
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 25; 255; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /ground_truth
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 42
+      Name: Odometry
+      Position Tolerance: 0.30000001192092896
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 10
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /nonsmooth_backstep_controller/odom
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -360,7 +461,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 3.0386643409729004
+      Distance: 6.876165390014648
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -375,10 +476,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.10960151255130768
+      Pitch: 0.7303981184959412
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.160394310951233
+      Yaw: 0.7503947019577026
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/twil_description/xacro/gps.urdf.xacro b/twil_description/xacro/gps.urdf.xacro
new file mode 100644
index 0000000..b36ceae
--- /dev/null
+++ b/twil_description/xacro/gps.urdf.xacro
@@ -0,0 +1,48 @@
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+<xacro:macro name="sensor_gps" params="name parent *origin">
+
+  <link name="${name}_link" />
+
+  <joint name="${name}_joint" type="fixed">
+	<parent link="${parent}"/>
+	<child link="${name}_link" />
+	<xacro:insert_block name="origin" />
+  </joint>
+
+  <gazebo reference="${name}_link">
+	<sensor name="${name}_sensor" type="gps">
+		<always_on>true</always_on>
+		<update_rate>5.0</update_rate>
+		<gps>
+			<position_sensing>
+				<horizontal>
+					<noise type="gaussian">
+						<mean>0.0</mean>
+						<stddev>2e-4</stddev>
+					</noise>
+				</horizontal>
+				<vertical>
+					<noise type="gaussian">
+						<mean>0.0</mean>
+						<stddev>2e-4</stddev>
+					</noise>
+				</vertical>
+			</position_sensing>
+		</gps>
+	
+		<plugin name="${name}_controller" filename="libgazebo_ros_gps_sensor.so">
+			<ros>
+				<!-- <namespace>/namespace</namespace> -->
+				<remapping>${name}_controller/out:=${name}/fix</remapping>
+			</ros>
+			<frame_name>${name}_link</frame_name>
+		</plugin>
+	</sensor>
+ 	<material>Gazebo/Grey</material>
+  </gazebo>
+ </xacro:macro>
+
+</robot>
diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro
index 1d82e71..5af782c 100644
--- a/twil_description/xacro/twil.urdf.xacro
+++ b/twil_description/xacro/twil.urdf.xacro
@@ -160,13 +160,22 @@
 	</plugin>
 
 	<plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
-		<frame_name>world</frame_name>
+		<frame_name>map</frame_name>
 		<body_name>twil_origin</body_name>
-		<argument>odom:=ground_truth</argument>
-		<!--update_rate>30</update_rate-->
+		<ros>
+			<remapping>odom:=ground_truth</remapping>
+		</ros>
+		<!--update_rate>100</update_rate-->
 	</plugin>
+
   </gazebo>
 
+  <xacro:if value="$(arg gps)">
+	<xacro:include filename="$(find twil_description)/xacro/gps.urdf.xacro"/>
+	<xacro:sensor_gps name="gps" parent="twil_origin">
+		<origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
+	</xacro:sensor_gps>
+  </xacro:if>
 
   <xacro:if value="$(arg d435)">
     <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />