BarceloChristian · eborghi10 · Apr 8, 2020 · Apr 8, 2020 · Apr 10, 2020 · Apr 10, 2020
diff --git a/.travis.yml b/.travis.yml
@@ -42,7 +42,8 @@ script:
   # Run the tests, ensuring the path is set correctly.
   - source devel/setup.bash
   # Run tests
-  - catkin_make run_tests && catkin_test_results
+  # NOTE: GAZEBO TESTS FAIL IN PARALLEL
+  - catkin_make run_tests -j1 && catkin_test_results
   # Lint package files ONLY for create_autonomy
   - sudo apt-get install python-catkin-lint
   - catkin lint -W2 --strict --explain $CREATE_AUTONOMY_SRC --ignore plugin_missing_install --ignore target_name_collision

diff --git a/ca_bringup/launch/minimal.launch b/ca_bringup/launch/minimal.launch
@@ -3,7 +3,7 @@
   <arg name="ns"        value="create$(arg robot_id)" doc="Namespace of the robot. By default: create1"/>
 
   <arg name="raspicam_receiver_IP" default="" doc="Raspicam client IP"/>
-  <arg name="laser" doc="Laser for mapping the environment"/>
+  <arg name="laser" default="$(optenv LASER)" doc="Laser for mapping the environment"/>
 
   <include file="$(find ca_node)/launch/create_2.launch"/>
 

diff --git a/ca_description/urdf/common_properties.xacro b/ca_description/urdf/common_properties.xacro
@@ -65,6 +65,6 @@
     </inertial>
   </xacro:macro>
 
-  <xacro:property name="cm_to_meters" value="${1/100}"/>
+  <xacro:property name="cm_to_meters" value="${1./100.}"/>
 
 </robot>
diff --git a/ca_description/urdf/create_base.xacro b/ca_description/urdf/create_base.xacro
@@ -62,19 +62,19 @@
     </xacro:wall_sensor>
 
     <xacro:cliff_sensor name="side_left_cliff_sensor">
-      <origin xyz="0.07 0.14 0.01" rpy="0 ${pi/2 - 0.09} 0" />
+      <origin xyz="0.07 0.14 -0.01" rpy="0 ${pi/2 - 0.09} 0"/>
     </xacro:cliff_sensor>
 
     <xacro:cliff_sensor name="side_right_cliff_sensor">
-      <origin xyz="0.07 -0.14 0.01" rpy="0 ${pi/2 - 0.09} 0" />
+      <origin xyz="0.07 -0.14 -0.01" rpy="0 ${pi/2 - 0.09} 0"/>
     </xacro:cliff_sensor>
 
     <xacro:cliff_sensor name="front_left_cliff_sensor">
-      <origin xyz="0.15 0.04 0.01" rpy="0 ${pi/2 - 0.09} 0" />
+      <origin xyz="0.15 0.04 -0.01" rpy="0 ${pi/2 - 0.09} 0"/>
     </xacro:cliff_sensor>
 
     <xacro:cliff_sensor name="front_right_cliff_sensor">
-      <origin xyz="0.15 -0.04 0.01" rpy="0 ${pi/2 - 0.09} 0" />
+      <origin xyz="0.15 -0.04 -0.01" rpy="0 ${pi/2 - 0.09} 0"/>
     </xacro:cliff_sensor>
 
     <xacro:imu_sensor name="imu">
@@ -86,10 +86,10 @@
     </xacro:omni_sensor>
 
     <xacro:wheel name="wheel_left" radius="${wheel_radius}" width="${wheel_width}">
-      <origin xyz="0 ${wheel_separation/2} 0" rpy="0 ${pi/2} ${pi/2}"/>
+      <origin xyz="0 ${wheel_separation/2} ${base_offset_z}" rpy="0 ${pi/2} ${pi/2}"/>
     </xacro:wheel>
     <xacro:wheel name="wheel_right" radius="${wheel_radius}" width="${wheel_width}">
-      <origin xyz="0 ${-wheel_separation/2} 0" rpy="0 ${pi/2} ${pi/2}"/>
+      <origin xyz="0 ${-wheel_separation/2} ${base_offset_z}" rpy="0 ${pi/2} ${pi/2}"/>
     </xacro:wheel>
 
     <xacro:property name="caster_z" value="${-caster_radius}"/>

diff --git a/ca_description/urdf/create_base_gazebo.xacro b/ca_description/urdf/create_base_gazebo.xacro
@@ -57,7 +57,7 @@
       <maxContacts>10</maxContacts>
       <sensor name='bumper_contact_sensor' type='contact'>
         <always_on>true</always_on>
-        <update_rate>50</update_rate>
+        <update_rate>100</update_rate>
         <contact>
           <collision>bumper_link_collision</collision>
         </contact>
@@ -66,15 +66,18 @@
           <frameName>base_footprint</frameName>
           <robotNamespace>${robot_name}</robotNamespace>
           <topicName>bumper</topicName>
+          <updateRate>20</updateRate>
         </plugin>
       </sensor>
     </gazebo>
 
     <!-- Cliff Publisher detector -->
     <gazebo>
       <plugin name="cliff_msg_publisher" filename="libcliff_msg_publisher.so">
+        <frameName>base_footprint</frameName>
+        <robotNamespace>${robot_name}</robotNamespace>
+        <topicName>cliff</topicName>
         <updateRate>10</updateRate>
-        <robotNumber>${robot_name}</robotNumber>
       </plugin>
     </gazebo>
 

diff --git a/ca_description/urdf/create_wheel.xacro b/ca_description/urdf/create_wheel.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
-<xacro:macro name="wheel" params="name:=wheel parent:=base_link radius width *origin">
+<xacro:macro name="wheel" params="name:=wheel parent:=base_footprint radius width *origin">
 
   <xacro:include filename="$(find ca_description)/urdf/common_properties.xacro"/>
 

diff --git a/ca_description/urdf/sensors/cliff_sensors.xacro b/ca_description/urdf/sensors/cliff_sensors.xacro
@@ -8,8 +8,9 @@
   <xacro:property name="parent_link"        value="${parent}"/>
   <xacro:property name="link_name"          value="${name}_link"/>
 
-  <xacro:property name="min_range_m"        value="0.01"/>
-  <xacro:property name="max_range_m"        value="0.04"/>
+  <xacro:property name="min_range_cm"       value="0.5"/>
+  <xacro:property name="max_range_cm"       value="5"/>
+  <xacro:property name="resolution_cm"      value="0.1"/>
 
   <xacro:property name="rate_hz"            value="20.0"/>
   <xacro:property name="min_cliff_value_cm" value="3.85"/>
@@ -23,7 +24,7 @@
   <link name="${link_name}" />
 
   <gazebo reference="${link_name}">
-    <sensor type="ray" name="${name}_sensor">
+    <sensor type="ray" name="${name}">
       <always_on>true</always_on>
       <update_rate>${rate_hz}</update_rate>
       <visualize>$(arg visualize)</visualize>
@@ -37,9 +38,9 @@
           </horizontal>
         </scan>
         <range>
-          <min>${min_range_m}</min>
-          <max>${max_range_m}</max>
-          <resolution>0.1</resolution>
+          <min>${min_range_cm*cm_to_meters}</min>
+          <max>${max_range_cm*cm_to_meters}</max>
+          <resolution>${resolution_cm*cm_to_meters}</resolution>
         </range>
       </ray>
       <plugin name="gazebo_ros_${name}" filename="libcreate_cliff_plugin.so">

diff --git a/ca_description/urdf/sensors/lasers/sim_3d_sensor.xacro b/ca_description/urdf/sensors/lasers/sim_3d_sensor.xacro
@@ -12,7 +12,7 @@
         <camera>
           <horizontal_fov>${60.0*pi/180.0}</horizontal_fov>
           <image>
-            <format>B8G8R8</format>
+            <format>R8G8B8</format>
             <width>640</width>
             <height>480</height>
           </image>
@@ -30,7 +30,7 @@
           <pointCloudTopicName>depth/points</pointCloudTopicName>
           <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
           <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
-          <frameName>${name}_depth_optical_frame</frameName>
+          <frameName>${name}_depth_frame</frameName>
           <baseline>0.1</baseline>
           <distortion_k1>0.0</distortion_k1>
           <distortion_k2>0.0</distortion_k2>

diff --git a/ca_description/urdf/sensors/wall_sensor.xacro b/ca_description/urdf/sensors/wall_sensor.xacro
@@ -8,7 +8,7 @@
   <xacro:property name="parent_link" value="${parent}"/>
   <xacro:property name="link_name"   value="${name}_link"/>
 
-  <xacro:property name="topic_name"  value="/wall_sensor/scan"/>
+  <xacro:property name="topic_name"  value="wall"/>
 
   <xacro:property name="rate_hz"     value="20.0"/>
   <xacro:property name="min_range_m" value="0.0160"/>

diff --git a/ca_driver/include/create_driver/create_driver.h b/ca_driver/include/create_driver/create_driver.h
@@ -132,14 +132,11 @@ class CreateDriver
   std::string tf_prefix_;
 
   const std::string str_base_footprint_ = "base_footprint";
-  const std::string str_base_link_ = "base_link";
   const std::string str_odom_ = "odom";
 
   const std::string str_wheel_left_link_  = "wheel_left_link";
   const std::string str_wheel_right_link_ = "wheel_right_link";
 
-  const double wheels_distance_ = 0.235;  // [m]
-
   void cmdVelCallback(const geometry_msgs::TwistConstPtr& msg);
   void debrisLEDCallback(const std_msgs::BoolConstPtr& msg);
   void spotLEDCallback(const std_msgs::BoolConstPtr& msg);

diff --git a/ca_driver/src/create_driver.cpp b/ca_driver/src/create_driver.cpp
@@ -540,7 +540,7 @@ void CreateDriver::publishWheelTf(
   tf_broadcaster_.sendTransform(
     tf::StampedTransform(
       tf, ros::Time::now(),
-      tf::resolve(tf_prefix_, str_base_link_),
+      tf::resolve(tf_prefix_, str_base_footprint_),
       frame_id));
 }
 
@@ -559,16 +559,17 @@ void CreateDriver::publishJointState()
 
   if (publish_tf_)
   {
+    const float distToBase = model_.getAxleLength();
     publishWheelTf(
         tf_wheel_left_,
         totalLeftAngDist,
         tf::resolve(tf_prefix_, str_wheel_left_link_),
-        tf::Vector3(0, wheels_distance_/2.0, 0));
+        tf::Vector3(0, distToBase/2.0, wheelRadius));
     publishWheelTf(
         tf_wheel_right_,
         totalRightAngDist,
         tf::resolve(tf_prefix_, str_wheel_right_link_),
-        tf::Vector3(0, -wheels_distance_/2.0, 0));
+        tf::Vector3(0, -distToBase/2.0, wheelRadius));
   }
 
   wheel_joint_pub_.publish(joint_state_msg_);

diff --git a/ca_gazebo/CMakeLists.txt b/ca_gazebo/CMakeLists.txt
@@ -12,12 +12,17 @@ find_package(catkin REQUIRED
     gazebo_ros
     roscpp
     roslint
+    tf
     tf2
     tf2_ros
 )
 
 set(trajectory_actor_plugin_name TrajectoryActorPlugin)
 
+###################################
+## catkin specific configuration ##
+###################################
+
 catkin_package(
 	INCLUDE_DIRS include
 	LIBRARIES
@@ -30,6 +35,10 @@ catkin_package(
     ca_msgs
 )
 
+###########
+## Build ##
+###########
+
 include_directories(
   include
 	${catkin_INCLUDE_DIRS}
@@ -181,12 +190,15 @@ add_dependencies(world_time_publisher ${catkin_EXPORTED_TARGETS})
 target_link_libraries(world_time_publisher
 	${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
 
-
+#############
+## Install ##
+#############
 
 install(DIRECTORY
 			launch
 			media
 			models
+      test
 			worlds
 		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
@@ -208,5 +220,16 @@ install(TARGETS
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+#############
+## Testing ##
+#############
+
 set(ROSLINT_CPP_OPTS "--filter=-build/c++11, -whitespace/braces")
 roslint_cpp()
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  # Tests list
+  add_rostest(test/hz.test)
+  add_rostest(test/publish.test)
+endif()
diff --git a/ca_gazebo/include/ca_gazebo/cliff_msg_publisher.h b/ca_gazebo/include/ca_gazebo/cliff_msg_publisher.h
@@ -6,6 +6,7 @@
 #define CA_GAZEBO_CLIFF_MSG_PUBLISHER_H
 
 #include <ros/ros.h>
+#include <tf/tf.h>
 
 #include <gazebo/gazebo.hh>
 #include <gazebo/physics/physics.hh>

diff --git a/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h b/ca_gazebo/include/ca_gazebo/create_bumper_plugin.h
@@ -79,7 +79,6 @@ class CreateBumperPlugin : public SensorPlugin
   ros::Publisher contact_pub_;
   ros::Subscriber gts_sub_;
   ca_msgs::Bumper bumper_event_;
-  void PublishBumperMsg();
 
   void GtsCb(const nav_msgs::Odometry::ConstPtr& msg);
   double robot_heading_;
@@ -100,6 +99,12 @@ class CreateBumperPlugin : public SensorPlugin
   /// \brief for setting ROS namespace
   std::string robot_namespace_;
 
+  /// \brief Update period of the bumper state
+  ros::Duration update_period_;
+
+  /// \brief Last time updated
+  ros::Time prev_update_time_;
+
   // Pointer to the update event connection
   event::ConnectionPtr update_connection_;
 };

diff --git a/ca_gazebo/launch/create_empty_world.launch b/ca_gazebo/launch/create_empty_world.launch
@@ -1,10 +1,10 @@
 <launch>
 	<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"/>
 
 	<arg name="paused" value="$(optenv PAUSED false)" doc="Start the simulation paused"/>
-	<arg name="gui"    value="$(optenv GUI true)" doc="Start gzclient (Gazebo's GUI)"/>
 	<arg name="debug"  value="$(optenv DEBUG false)" doc="Enable debugging with gdb"/>
 
 	<!-- Launch Gazebo world -->

diff --git a/ca_gazebo/launch/create_maze.launch b/ca_gazebo/launch/create_maze.launch
@@ -1,5 +1,7 @@
 <!-- https://github.com/tu-darmstadt-ros-pkg/hector_nist_arenas_gazebo -->
 <launch>
+  <arg name="gui" default="$(optenv GUI true)" doc="Start gzclient (Gazebo's GUI)"/>
+
   <param name="create1/amcl/initial_pose_x" value="5"/>
   <param name="create1/amcl/initial_pose_y" value="5"/>
   <param name="create1/amcl/initial_pose_a" value="1.5708"/>
@@ -10,5 +12,6 @@
 
 	<include file="$(find ca_gazebo)/launch/create_empty_world.launch">
     <arg name="env" value="maze"/>
+    <arg name="gui" value="$(arg gui)"/>
 	</include>
-</launch>
+</launch>
diff --git a/ca_gazebo/package.xml b/ca_gazebo/package.xml
@@ -16,6 +16,7 @@
   <build_depend>gazebo_ros</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>roslint</build_depend>
+  <build_depend>tf</build_depend>
   <build_depend>tf2</build_depend>
   <build_depend>tf2_ros</build_depend>
 
@@ -25,10 +26,13 @@
   <run_depend>gazebo_plugins</run_depend>
   <run_depend>gazebo_ros</run_depend>
   <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
   <run_depend>tf2</run_depend>
   <run_depend>tf2_ros</run_depend>
   <run_depend>xacro</run_depend>
 
+  <test_depend>rostest</test_depend>
+
   <export>
     <gazebo_ros gazebo_media_path="${prefix}
                                   :${prefix}/media