Merge pull request #22 from ori-drs/noetic

Noetic compatibility / tf2 upgrade
ori-drs · Feb 23, 2021 · 73481a6 · 73481a6
2 parents 59a345c + 5550af1
commit 73481a6
Show file tree

Hide file tree

Showing 18 changed files with 57 additions and 142 deletions.
diff --git a/filter_tools/CMakeLists.txt b/filter_tools/CMakeLists.txt
@@ -1,7 +1,7 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(filter_tools)
 
-add_compile_options(-std=c++11)
+set(CMAKE_CXX_STANDARD 14)
 
 find_package(catkin REQUIRED)
 find_package(Eigen3)

diff --git a/pronto_biped_core/CMakeLists.txt b/pronto_biped_core/CMakeLists.txt
@@ -1,7 +1,7 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(pronto_biped_core)
 
-add_compile_options(-std=c++11)
+set(CMAKE_CXX_STANDARD 14)
 
 find_package(catkin REQUIRED COMPONENTS filter_tools
                                         pronto_core)

diff --git a/pronto_biped_ros/CMakeLists.txt b/pronto_biped_ros/CMakeLists.txt
@@ -1,7 +1,7 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(pronto_biped_ros)
 
-add_compile_options(-std=c++11)
+set(CMAKE_CXX_STANDARD 14)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

diff --git a/pronto_core/CMakeLists.txt b/pronto_core/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 
 project(pronto_core)
 set(CMAKE_CXX_STANDARD 14)

diff --git a/pronto_msgs/CMakeLists.txt b/pronto_msgs/CMakeLists.txt
@@ -1,7 +1,7 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(pronto_msgs)
 
-add_compile_options(-std=c++11)
+set(CMAKE_CXX_STANDARD 14)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)

diff --git a/pronto_quadruped/CMakeLists.txt b/pronto_quadruped/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(pronto_quadruped)
 
 set(CMAKE_CXX_STANDARD 14)
@@ -48,10 +48,12 @@ include_directories(${catkin_INCLUDE_DIRS})
 find_package(Eigen3)
 include_directories(${EIGEN3_INCLUDE_DIRS})
 
+find_package(Boost REQUIRED COMPONENTS filesystem)
+
 catkin_package(LIBRARIES ${PROJECT_NAME} 
                INCLUDE_DIRS include
                CATKIN_DEPENDS pronto_quadruped_commons 
-               DEPENDS EIGEN3)
+               DEPENDS EIGEN3 Boost)
 
 add_library(${PROJECT_NAME} SHARED src/LegOdometer.cpp
                                    src/DynamicStanceEstimator.cpp
@@ -62,7 +64,7 @@ add_library(${PROJECT_NAME} SHARED src/LegOdometer.cpp
                                    src/DataLogger.cpp
                                    src/ImuBiasLock.cpp)
 
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${catkin_LIBRARIES})
 
 install(DIRECTORY include/${PROJECT_NAME}/
         DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})

diff --git a/pronto_quadruped/include/pronto_quadruped/ForceSensorStanceEstimator.hpp b/pronto_quadruped/include/pronto_quadruped/ForceSensorStanceEstimator.hpp
@@ -19,7 +19,7 @@ class ForceSensorStanceEstimator : public StanceEstimatorBase {
 
   void setGRF(const LegVectorMap& grf) override;
 
-  bool isStance(LegID leg) const;
+  bool isStance(LegID leg) const override;
 
 protected:
   LegBoolMap stance_;

diff --git a/pronto_quadruped/include/pronto_quadruped/LegOdometer.hpp b/pronto_quadruped/include/pronto_quadruped/LegOdometer.hpp
@@ -91,7 +91,7 @@ class LegOdometer  : public LegOdometerBase {
                       Vector3d& position,
                       Matrix3d& pos_covariance,
                       Quaterniond& orientation,
-                      Matrix3d& orient_covariance) {
+                      Matrix3d& orient_covariance) override {
         std::cerr << "Function not implemented yet!" << std::endl;
 	return false;
     }
@@ -103,17 +103,19 @@ class LegOdometer  : public LegOdometerBase {
                           const LegBoolMap& stance_legs,
                           const LegScalarMap& stance_prob,
                           Vector3d& velocity,
-                          Matrix3d& covariance);
+                          Matrix3d& covariance) override;
 
     inline void getVelocity(Vector3d& velocity, Matrix3d& covariance) override
     {
         velocity = xd_b_;
         covariance = vel_cov_;
     }
-    inline void getPosition(Vector3d& position, Matrix3d& covariance){
+    inline void getPosition(Vector3d& position, Matrix3d& covariance) override
+    {
         std::cerr << "Function not implemented yet!" << std::endl;
     }
-    inline void getOrientation(Quaterniond& orientation, Matrix3d& covariance){
+    inline void getOrientation(Quaterniond& orientation, Matrix3d& covariance) override
+    {
         std::cerr << "Function not implemented yet!" << std::endl;
     }
 

diff --git a/pronto_quadruped/src/ForceSensorStanceEstimator.cpp b/pronto_quadruped/src/ForceSensorStanceEstimator.cpp
@@ -38,5 +38,3 @@ void ForceSensorStanceEstimator::setGRF(const LegVectorMap& grf) {
 bool ForceSensorStanceEstimator::isStance(LegID leg) const {
   return stance_[leg];
 }
-
-
diff --git a/pronto_quadruped_commons/CMakeLists.txt b/pronto_quadruped_commons/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Project configuration
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.0.2)
 project(pronto_quadruped_commons)
 
 #set(HEADER_INSTALL_ROOT /usr/local/include/ CACHE PATH "Where to install the iit-commons headers")

diff --git a/pronto_quadruped_ros/CMakeLists.txt b/pronto_quadruped_ros/CMakeLists.txt
@@ -1,15 +1,16 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(pronto_quadruped_ros)
 
 set(CMAKE_CXX_STANDARD 14)
 
 find_package(catkin REQUIRED COMPONENTS roscpp
                                         pronto_core
                                         pronto_ros
-                                        pronto_quadruped                                        
+                                        pronto_quadruped
                                         geometry_msgs
                                         sensor_msgs
-                                        pronto_msgs)
+                                        pronto_msgs
+                                        visualization_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -106,8 +107,7 @@ catkin_package(INCLUDE_DIRS include
                               geometry_msgs
                               sensor_msgs
                               pronto_msgs
-               #  DEPENDS system_lib
-               )
+                              visualization_msgs)
 
 ###########
 ## Build ##

diff --git a/pronto_quadruped_ros/include/pronto_quadruped_ros/bias_lock_handler_ros.hpp b/pronto_quadruped_ros/include/pronto_quadruped_ros/bias_lock_handler_ros.hpp
@@ -57,7 +57,6 @@ class ImuBiasLockBaseROS : public DualSensingModule<sensor_msgs::Imu, JointState
                           const RBIM &default_cov, RBIS &init_state,
                           RBIM &init_cov) override;
 
-  void processSecondaryMessage(const JointStateT &msg) = 0;
 protected:
   ros::NodeHandle& nh_;
   std::unique_ptr<quadruped::ImuBiasLock> bias_lock_module_;

diff --git a/pronto_quadruped_ros/package.xml b/pronto_quadruped_ros/package.xml
@@ -61,6 +61,7 @@
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>pronto_msgs</depend>
+  <depend>visualization_msgs</depend>
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->

diff --git a/pronto_ros/CMakeLists.txt b/pronto_ros/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(pronto_ros)
 
 set(CMAKE_CXX_STANDARD 14)
@@ -8,92 +8,20 @@ find_package(catkin REQUIRED COMPONENTS pronto_core
                                         sensor_msgs
                                         eigen_conversions
                                         tf_conversions
+                                        tf2_ros
                                         pronto_msgs
                                         geometry_msgs)
 
-# find_package(Boost REQUIRED COMPONENTS system)
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
-
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs  # Or other packages containing msgs
-# )
-
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
 catkin_package(INCLUDE_DIRS include
                LIBRARIES ${PROJECT_NAME}
                CATKIN_DEPENDS pronto_core
                               roscpp
                               sensor_msgs
                               tf_conversions
+                              tf2_ros
                               eigen_conversions
                               pronto_msgs
                               geometry_msgs
-               # DEPENDS system_lib
 )
 
 include_directories(include ${catkin_INCLUDE_DIRS})

diff --git a/pronto_ros/include/pronto_ros/gps_ros_handler.hpp b/pronto_ros/include/pronto_ros/gps_ros_handler.hpp
@@ -18,7 +18,7 @@ class GPSHandlerROS : public SensingModule<pronto_msgs::GPSData> {
                             const RBIS &default_state,
                             const RBIM &default_cov,
                             RBIS &init_state,
-                            RBIM &init_cov);
+                            RBIM &init_cov) override;
 
 protected:
     ros::NodeHandle nh_;

diff --git a/pronto_ros/include/pronto_ros/ros_frontend.hpp b/pronto_ros/include/pronto_ros/ros_frontend.hpp
@@ -7,9 +7,10 @@
 #include <sensor_msgs/Imu.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <geometry_msgs/TwistWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <tf_conversions/tf_eigen.h>
 #include <chrono>
-#include <tf/transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include <string>
 #include <cstdlib>
 #include <cxxabi.h>
@@ -88,6 +89,7 @@ class ROSFrontEnd {
                                                                cov,
                                                                RBISUpdateInterface::reset,
                                                                state.utime), true);
+        return true;
     }
     template <class MsgT>
     void initCallback(boost::shared_ptr<MsgT const> msg,
@@ -130,8 +132,8 @@ class ROSFrontEnd {
 
     ros::Publisher pose_pub_;
     ros::Publisher twist_pub_;
-    tf::TransformBroadcaster tf_broadcaster_;
-    tf::StampedTransform tf_pose_;
+    tf2_ros::TransformBroadcaster tf2_broadcaster_;
+    geometry_msgs::TransformStamped transform_msg_;
     bool publish_tf_ = false;
 
     geometry_msgs::PoseWithCovarianceStamped pose_msg_;
@@ -410,10 +412,20 @@ std::cerr << ":::::::" << std::endl;
             // fill in time
             pose_msg_.header.stamp = ros::Time().fromNSec(head_state.utime * 1000);
             if(publish_tf_){
-                tf_pose_.setOrigin(temp_v3);
-                tf_pose_.setRotation(temp_q);
-                tf_pose_.stamp_ = ros::Time::now();
-                tf_broadcaster_.sendTransform(tf_pose_);
+                // Only publish the pose if the timestamp is different:
+                // This prevents issues in Noetic where repeated warnings of type:
+                // "TF_REPEATED_DATA ignoring data with redundant timestamp for frame base at time"
+                // are otherwise printed to the terminal.
+                // Cf. https://github.com/ros/geometry2/issues/467#issuecomment-751572836
+                ros::Time new_stamp = ros::Time::now();
+                if (new_stamp > transform_msg_.header.stamp) {
+                    transform_msg_.transform.translation.x = pose_msg_.pose.pose.position.x;
+                    transform_msg_.transform.translation.y = pose_msg_.pose.pose.position.y;
+                    transform_msg_.transform.translation.z = pose_msg_.pose.pose.position.z;
+                    transform_msg_.transform.rotation = pose_msg_.pose.pose.orientation;
+                    transform_msg_.header.stamp = new_stamp;
+                    tf2_broadcaster_.sendTransform(transform_msg_);
+                }
             }
 
             // TODO insert appropriate covariance into the message

diff --git a/pronto_ros/package.xml b/pronto_ros/package.xml
@@ -7,7 +7,6 @@
   <author email="[email protected]">Marco Camurri</author>
   <maintainer email="[email protected]">Marco Camurri</maintainer>
 
-
   <license>LGPLv2.1</license>
 
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
@@ -21,27 +20,6 @@
   <!-- Example: -->
   <!-- <author email="[email protected]">Jane Doe</author> -->
 
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>pronto_core</build_depend>
   <build_depend>roscpp</build_depend>
@@ -51,13 +29,8 @@
   <exec_depend>roscpp</exec_depend>
   <depend>sensor_msgs</depend>
   <depend>tf_conversions</depend>
+  <depend>tf2_ros</depend>
   <depend>eigen_conversions</depend>
-<depend>pronto_msgs</depend>
-<depend>geometry_msgs</depend>
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
+  <depend>pronto_msgs</depend>
+  <depend>geometry_msgs</depend>
 </package>
Original file line number	Diff line number	Diff line change
Expand Up		@@ -38,5 +38,3 @@ void ForceSensorStanceEstimator::setGRF(const LegVectorMap& grf) {
		bool ForceSensorStanceEstimator::isStance(LegID leg) const {
		return stance_[leg];
		}