Localization improvements with robot_localization

ca_bringup/launch/complete.launch

@@ -20,6 +20,9 @@
       <arg name="robot_id"     value="$(arg robot_id)"/>
     </include>
 
+    <!-- Localization node -->
+  <include file="$(find ca_localization)/launch/localize.launch"/>
+
     <!-- Navigation stack -->
     <include file="$(find ca_move_base)/launch/navigate.launch">
       <arg name="env"          value="$(arg env)"/>

ca_bringup/launch/minimal.launch

@@ -8,7 +8,9 @@
   <arg name="rviz" default="$(optenv RVIZ false)"     doc="Flag to use RViz"/>
   <arg name="imu"  default="$(optenv IMU true)"       doc="Flag to use the IMU MPU9255"/>
 
-  <include file="$(find ca_node)/launch/create_2.launch" unless="$(arg ros_control)"/>
+  <param name="tf_prefix" value="create$(arg robot_id)_tf" />
+
+  <include file="$(find ca_node)/launch/create_2.launch"               unless="$(arg ros_control)"/>
   <include file="$(find ca_bringup)/launch/minimal_ros_control.launch" if="$(arg ros_control)"/>
 
   <!-- RViz -->

ca_description/urdf/create_base_gazebo.xacro

@@ -4,6 +4,7 @@
 
     <gazebo>
       <xacro:arg name="enable_ros_control" default="False"/>
+
       <!-- Diff-drive controller -->
       <xacro:unless value="$(arg enable_ros_control)">
         <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
@@ -21,12 +22,12 @@
           <odometryFrame>odom</odometryFrame>
           <robotBaseFrame>base_footprint</robotBaseFrame>
           <publishWheelTF>true</publishWheelTF>
-          <publishOdomTF>true</publishOdomTF>
+          <publishOdomTF>false</publishOdomTF>
           <publishWheelJointState>true</publishWheelJointState>
-          <odometrySource>world</odometrySource>
+          <odometrySource>encoder</odometrySource>
           <publishTf>true</publishTf>
         </plugin>
-    </xacro:unless>
+      </xacro:unless>
 
       <!-- ros_control plugin -->
       <xacro:if value="$(arg enable_ros_control)">
@@ -36,6 +37,7 @@
         <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
       </plugin>
       </xacro:if>
+
       <!-- Ground truth -->
       <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
         <alwaysOn>true</alwaysOn>

ca_description/urdf/stacks/turtlebot.xacro

@@ -13,7 +13,7 @@
   <xacro:property name="link_name" value="pole_${name}_link"/>
   <joint name="pole_${name}_joint" type="fixed">
     <xacro:insert_block name="origin"/>
-    <parent link="${parent}"/>
+    <parent link="${parent}" rpy="0 0 ${pi}"/>
     <child link="${link_name}"/>
   </joint>
   <link name="${link_name}">

ca_driver/config/default.yaml

@@ -20,4 +20,4 @@ latch_cmd_duration: 0.2
 loop_hz: 30.0
 
 # Whether to publish the transform between odom_frame and base_frame
-publish_tf: true
+publish_tf: false

ca_gazebo/launch/create_empty_world.launch

@@ -1,8 +1,10 @@
 <launch>
-	<arg name="env" default="empty" doc="Name of the environment"/>
+	<arg name="env" default="empty"              doc="Name of the environment"/>
 	<arg name="gui" default="$(optenv GUI true)" doc="Start gzclient (Gazebo's GUI)"/>
+
 	<arg name="localization" default="$(optenv LOCALIZATION none)"
-       doc="Localization type. More options here: https://github.com/RoboticaUtnFrba/create_autonomy/wiki/Launching-options"/>
+       doc="Localization type. More options here:
+            https://github.com/RoboticaUtnFrba/create_autonomy/wiki/Launching-options"/>
 
 	<arg name="paused"  value="$(optenv PAUSED false)"  doc="Start the simulation paused"/>
 	<arg name="debug"   value="$(optenv DEBUG false)" 	doc="Enable debugging with gdb"/>

ca_gazebo/launch/spawn_create.launch

@@ -1,6 +1,11 @@
 <launch>
   <arg name="robot_id"  default="$(optenv ID 1)"      doc="Unique identifier of the robot [1-Inf.)"/>
-	<arg name="enable_ros_control" value="$(optenv ROS_CONTROL false)" doc="Enable ros_control"/>
+  <arg name="ns"        value="create$(arg robot_id)" doc="Namespace of the robot. By default: create1"/>
+  <arg name="tf_prefix" value="$(arg ns)_tf"          doc="Tf prefix"/>
+
+  <arg name="enable_ros_control" value="$(optenv ROS_CONTROL false)" doc="Enable ros_control"/>
+  <arg name="use_nodelets"       default="$(optenv NODELET true)"
+			 doc="Use nodelets: http://wiki.ros.org/nodelet" />
 
   <!-- Create 2 description -->
   <include file="$(find ca_description)/launch/create_description.launch">
@@ -15,6 +20,10 @@
     <arg name="robot_id" value="$(arg robot_id)"/>
   </include>
 
+  <!-- ros_control -->
 	<include file="$(find create_control)/launch/create_control.launch" if="$(arg enable_ros_control)"/>
 
+  <!-- Nodelet manager -->
+	<node pkg="nodelet" type="nodelet" name="nodelet_manager"  args="manager"/>
+
 </launch>

ca_gazebo/launch/spawn_multirobot.launch

@@ -1,6 +1,7 @@
 <launch>
   <!-- https://answers.ros.org/question/229489/how-do-i-create-dynamic-launch-files/ -->
-  <arg name="nr" default="$(optenv NUM_ROBOTS 1)"/>
+  <arg name="nr" default="$(optenv NUM_ROBOTS 1)" doc="Unique identifier of the robot [1-Inf.)"/>
+  <arg name="ns" value="create$(arg nr)"          doc="Namespace of the robot. By default: create1"/>
 
   <arg name="env" default="empty" doc="Name of the environment"/>
   <arg name="localization" default="$(optenv LOCALIZATION none)"
@@ -9,14 +10,15 @@
   <arg name="rviz" default="$(optenv RVIZ false)" doc="Flag to use RViz"/>
 
   <!-- Start 1 robot -->
-  <group ns="$(eval 'create' + str(arg('nr')))">
+  <group ns="$(arg ns)">
     <include file="$(find ca_gazebo)/launch/spawn_create.launch">
       <arg name="robot_id" value="$(arg nr)"/>
     </include>
 
-    <param name="tf_prefix" value="create$(arg nr)_tf" />
+    <!-- Localization -->
+    <include file="$(find ca_localization)/launch/localize.launch"/>
 
-    <!-- localization -->
+    <!-- Navigation -->
     <include if="$(eval str(arg('localization'))!='none')"
              file="$(find ca_move_base)/launch/navigate.launch">
       <arg name="env"          value="$(arg env)"/>

ca_node/launch/create_2.launch

@@ -8,7 +8,9 @@
 
   <node name="ca_driver" pkg="ca_driver" type="ca_driver" output="screen" required="true">
     <rosparam command="load" file="$(find ca_driver)/config/default.yaml"/>
-    <param name="namespace" value="$(arg ns)"/>
+
+    <param name="namespace"  value="$(arg ns)"/>
+    <param name="tf_prefix"  value="$(arg tf_prefix)"/>
   </node>
 
   <!-- Robot description -->

ca_tools/scripts/move_square.py

@@ -22,8 +22,8 @@
 
 class SquareMove(object):
     """
-    This class is an abstract class to control a square trajectory on the turtleBot.
-    It mainly declare and subscribe to ROS topics in an elegant way.
+    This class is an abstract class to control a square trajectory on the robot.
+    It mainly declares and subscribes to ROS topics in an elegant way.
     """
 
     def __init__(self):
@@ -58,19 +58,19 @@ def start_ros(self):
     def stop_robot(self):
 
         # Get the initial time
-        self.t_init = time.time()
+        self.t_init = rospy.get_time()
 
         # We publish for a second to be sure the robot receive the message
-        while time.time() - self.t_init < 1 and not rospy.is_shutdown():
+        while rospy.get_time() - self.t_init < 1 and not rospy.is_shutdown():
 
             self.vel_ros_pub(Twist())
-            time.sleep(self.pub_rate)
+            rospy.sleep(self.pub_rate)
 
     def move(self):
         """ To be surcharged in the inheriting class"""
 
         while not rospy.is_shutdown():
-            time.sleep(1)
+            rospy.sleep(1)
 
     def __odom_ros_sub(self, msg):
 
@@ -84,12 +84,10 @@ def vel_ros_pub(self, msg):
 class SquareMoveVel(SquareMove):
     """
     This class implements a open-loop square trajectory based on velocity control. HOWTO:
-     - Start the roscore (on the computer or the robot, depending on your configuration)
-            $ roscore
-     - Start the sensors on the turtlebot:
-            $ roslaunch turtlebot3_bringup turtlebot3_robot.launch
+     - Start the sensors on the robot:
+            $ roslaunch ca_bringup minimal.launch
      - Start this node on your computer:
-            $ python move_square vel
+            $ roslaunch ca_tools move_square.launch args:=vel
     """
 
     def __init__(self, dir):
@@ -100,53 +98,51 @@ def __init__(self, dir):
     def go_forward(self, duration, speed):
 
         # Get the initial time
-        self.t_init = time.time()
+        self.t_init = rospy.get_time()
 
         # Set the velocity forward and wait (do it in a while loop to keep publishing the velocity)
-        while time.time() - self.t_init < duration and not rospy.is_shutdown():
+        while rospy.get_time() - self.t_init < duration and not rospy.is_shutdown():
 
             msg = Twist()
             msg.linear.x = speed
             msg.angular.z = 0
             self.vel_ros_pub(msg)
-            time.sleep(self.pub_rate)
+            rospy.sleep(self.pub_rate)
 
     def turn(self, duration, ang_speed):
 
         # Get the initial time
-        self.t_init = time.time()
+        self.t_init = rospy.get_time()
 
         # Set the velocity forward and wait 2 sec (do it in a while loop to keep publishing the velocity)
-        while time.time() - self.t_init < duration and not rospy.is_shutdown():
+        while rospy.get_time() - self.t_init < duration and not rospy.is_shutdown():
 
             msg = Twist()
             msg.linear.x = 0
             msg.angular.z = ang_speed
             self.vel_ros_pub(msg)
-            time.sleep(self.pub_rate)
+            rospy.sleep(self.pub_rate)
 
     def move(self):
 
-        self.go_forward(2, 0.33)
-        self.turn(self.dir*3.5, 0.33)
-        self.go_forward(2, 0.33)
-        self.turn(self.dir*3.5, 0.33)
-        self.go_forward(2, 0.33)
-        self.turn(self.dir*3.5, 0.33)
-        self.go_forward(2, 0.33)
+        self.go_forward(4, 0.25)
+        self.turn(self.dir*3.5, 0.4488)
+        self.go_forward(4, 0.25)
+        self.turn(self.dir*3.5, 0.4488)
+        self.go_forward(4, 0.25)
+        self.turn(self.dir*3.5, 0.4488)
+        self.go_forward(4, 0.25)
         self.stop_robot()
 
 
 class SquareMoveOdom(SquareMove):
     """
     This class implements a semi closed-loop square trajectory based on relative position control,
-    where only odometry is used. HOWTO:
-     - Start the roscore (on the computer or the robot, depending on your configuration)
-            $ roscore
-     - Start the sensors on the turtlebot:
-            $ roslaunch turtlebot3_bringup turtlebot3_robot.launch
+    where only wheel odometry is used. HOWTO:
+     - Start the sensors on the robot:
+            $ roslaunch ca_bringup minimal.launch
      - Start this node on your computer:
-            $ python move_square odom
+            $ roslaunch ca_tools move_square.launch args:=odom
     """
 
     def __init__(self):
@@ -184,42 +180,48 @@ def move_of(self, d, speed=0.5):
             msg.linear.x = speed
             msg.angular.z = 0
             self.vel_ros_pub(msg)
-            time.sleep(self.pub_rate)
+            rospy.sleep(self.pub_rate)
 
         rospy.loginfo("\n")
 
+    def get_angle_diff(self, a1, a2):
+
+        # Wraps the angle difference
+        diff = a1 - a2
+        return math.atan2(math.sin(diff), math.cos(diff))
+
     def turn_of(self, a, ang_speed=0.4):
 
         # Convert the orientation quaternion message to Euler angles
         a_init = self.get_z_rotation(self.odom_pose.orientation)
         print (a_init)
 
         # Set the angular velocity forward until angle is reached
-        while (self.get_z_rotation(self.odom_pose.orientation) - a_init) \
+        while (self.get_angle_diff(self.get_z_rotation(self.odom_pose.orientation), a_init)) \
                 < a and not rospy.is_shutdown():
 
             msg = Twist()
             msg.angular.z = ang_speed
             msg.linear.x = 0
             self.vel_ros_pub(msg)
-            time.sleep(self.pub_rate)
+            rospy.sleep(self.pub_rate)
 
         rospy.loginfo("\n")
 
     def move(self):
 
         # Wait that our python program has received its first messages
         while self.odom_pose is None and not rospy.is_shutdown():
-            time.sleep(0.1)
+            rospy.sleep(0.1)
 
         # Implement main instructions
-        self.move_of(0.33)
-        self.turn_of(math.pi/4)
-        self.move_of(0.33)
-        self.turn_of(math.pi/4)
-        self.move_of(0.33)
-        self.turn_of(math.pi/4)
-        self.move_of(0.33)
+        self.move_of(1.0)
+        self.turn_of(math.pi/2.)
+        self.move_of(1.0)
+        self.turn_of(math.pi/2.)
+        self.move_of(1.0)
+        self.turn_of(math.pi/2.)
+        self.move_of(1.0)
         self.stop_robot()
 
 

navigation/ca_localization/config/ekf_imu.yaml

@@ -0,0 +1,14 @@
+imu0: "imu/data"
+
+imu0_config: [
+              false, false, false,
+              false, false, false,
+              false, false, false,
+              false, false, true,
+              false, false, false
+             ]
+
+imu0_differential: false
+imu0_relative: true
+
+imu0_remove_gravitational_acceleration: true