Merge pull request #20 from ori-drs/remove-old-dependencies

get rid of pronto math and other old dependencies

README.md

@@ -45,9 +45,7 @@ quadruped robot. This is a fork of the `iit_commons` package (see
 - other support packages for filtering
 
 ## Dependencies
-Pronto depends on Eigen, Boost, and on the catkin packages in the following repositories (likely to be removed in future)
-- [common\_utils](https://github.com/ori-drs/common_utils)
-- [kinematics\_utils](https://github.com/ori-drs/kinematic_utils)
+Pronto depends on Eigen and Boost
 
 ## System Requirements
 The target operating system is **Ubuntu 18.04** equipped with **ROS Melodic**.  

filter_tools/CMakeLists.txt

@@ -39,4 +39,13 @@ target_link_libraries(${BACKLASH_FILTER_LIB} ${KALMAN_FILTER_LIB})
 #add_executable(test-filter src/filtering_demo.cpp)
 #target_link_libraries(test-filter ${catkin_LIBRARIES} backlash_filter_tools)
 
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME} ${KALMAN_FILTER_LIB} ${BACKLASH_FILTER_LIB}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
 

pronto_biped_core/CMakeLists.txt

@@ -4,7 +4,6 @@ project(pronto_biped_core)
 add_compile_options(-std=c++11)
 
 find_package(catkin REQUIRED COMPONENTS filter_tools
-                                        pronto_math
                                         pronto_core)
 
 
@@ -13,7 +12,6 @@ set(LEGODO_LIB_NAME pronto_biped_legodo)
 catkin_package(INCLUDE_DIRS include
                LIBRARIES ${LEGODO_LIB_NAME}
                CATKIN_DEPENDS filter_tools
-                              pronto_math
                               pronto_core)
 
 include_directories(include ${catkin_INCLUDE_DIRS})
@@ -29,3 +27,19 @@ add_library(${LEGODO_LIB_NAME} src/leg_estimate.cpp
 
 add_dependencies(${LEGODO_LIB_NAME} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(${LEGODO_LIB_NAME} ${catkin_LIBRARIES})
+
+install(TARGETS ${LEGODO_LIB_NAME}
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+catkin_install_python(PROGRAMS python/state-sync-simple.py
+                               python/test_pose_command.py
+                               python/send-a-pose.py
+                               python/state-sync-simple-multisense-standalone.py
+                               python/send-fake-imu.py
+                               python/send-fake-imu-ros.py
+                      DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

pronto_biped_core/include/pronto_biped_core/foot_contact_classify.hpp

@@ -10,7 +10,6 @@
 #include <Eigen/StdVector>
 
 #include <boost/shared_ptr.hpp>
-#include <pronto_math/pronto_math.hpp> // just for the Isometry3dTime
 #include <filter_tools/Filter.hpp>
 #include <filter_tools/SignalTap.hpp> // SchmittTrigger
 

pronto_biped_core/package.xml

@@ -14,7 +14,6 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <depend>filter_tools</depend>
-  <depend>pronto_math</depend>
   <depend>pronto_core</depend>
 
   <!-- The export tag contains other, unspecified, tags -->

pronto_biped_core/src/leg_estimate.cpp

@@ -513,7 +513,7 @@ float LegEstimator::updateOdometry(const std::vector<std::string>& joint_name,
       world_to_primary_foot_transition_ = world_to_primary_foot_slide_;
       world_to_primary_foot_transition_init_ = true;
       if (publish_diagnostics_) { // this was enabled by default for a long time
-        Isometry3dTime world_to_primary_trans_T = Isometry3dTime(current_utime_ , world_to_primary_foot_transition_ ) ;
+        // Isometry3dTime world_to_primary_trans_T = Isometry3dTime(current_utime_ , world_to_primary_foot_transition_ ) ;
         //TODO no pc vis anymore
         //pc_vis_->pose_to_lcm_from_list(1014, world_to_primary_trans_T);
       }

pronto_biped_core/src/legodo_common.cpp

@@ -1,4 +1,6 @@
 #include "pronto_biped_core/legodo_common.hpp"
+#include <pronto_core/rotations.hpp>
+#include <iostream>
 
 namespace pronto {
 namespace biped {
@@ -43,8 +45,8 @@ void LegOdoCommon::getCovariance(const LegOdometryMode &mode_current,
         R_legodo(4) = std::pow(R_legodo_vang_current, 2);
         R_legodo(5) = std::pow(R_legodo_vang_current, 2);
 
-        z_indices.head<3>() = eigen_utils::RigidBodyState::velocityInds();
-        z_indices.tail<3>() = eigen_utils::RigidBodyState::angularVelocityInds();
+        z_indices.head<3>() = RigidBodyState::velocityInds();
+        z_indices.tail<3>() = RigidBodyState::angularVelocityInds();
         break;
     case LegOdometryMode::LIN_RATE :
         z_indices.resize(3);
@@ -53,7 +55,7 @@ void LegOdoCommon::getCovariance(const LegOdometryMode &mode_current,
         R_legodo(1) = std::pow(R_legodo_vxyz_current, 2);
         R_legodo(2) = std::pow(R_legodo_vxyz_current, 2);
 
-        z_indices.head<3>() = eigen_utils::RigidBodyState::velocityInds();
+        z_indices.head<3>() = RigidBodyState::velocityInds();
         break;
     case LegOdometryMode::POSITION_AND_LIN_RATE :
         z_indices.resize(6);
@@ -65,8 +67,8 @@ void LegOdoCommon::getCovariance(const LegOdometryMode &mode_current,
         R_legodo(4) = std::pow(R_legodo_vxyz_current, 2);
         R_legodo(5) = std::pow(R_legodo_vxyz_current, 2);
 
-        z_indices.head<3>() = eigen_utils::RigidBodyState::positionInds();
-        z_indices.tail<3>() = eigen_utils::RigidBodyState::velocityInds();
+        z_indices.head<3>() = RigidBodyState::positionInds();
+        z_indices.tail<3>() = RigidBodyState::velocityInds();
         break;
     default:
         break;
@@ -80,14 +82,14 @@ LegOdoCommon::Transform LegOdoCommon::getTransAsVelocityTrans(const Transform &m
 {
   Transform msgT_vel(Transform::Identity());
 
-  Eigen::Vector3d rpy = eigen_utils::getEulerAngles(Eigen::Quaterniond(msgT.rotation()));
+  Eigen::Vector3d rpy = rotation::getEulerAngles(Eigen::Quaterniond(msgT.rotation()));
 
   double elapsed_time = ((double)(utime - prev_utime)) / 1e6;
   Eigen::Vector3d rpy_rate = rpy / elapsed_time;
 
   msgT_vel.translate(msgT.translation() / elapsed_time);
 
-  Eigen::Quaterniond quat_vel = eigen_utils::setQuatEulerAngles(rpy_rate);
+  Eigen::Quaterniond quat_vel = rotation::setQuatEulerAngles(rpy_rate);
 
   msgT_vel.rotate(quat_vel);
 
@@ -133,7 +135,7 @@ RBISUpdateInterface * LegOdoCommon::createMeasurement(const Transform &odo_posit
         // This was finished in Feb 2015 but not tested on the robot
         z_meas.resize(6);
 
-        Eigen::Vector3d rpy = eigen_utils::getEulerAngles(delta_odoT.rotation());
+        Eigen::Vector3d rpy = rotation::getEulerAngles(delta_odoT.rotation());
         double elapsed_time =  ((double)(utime -  prev_utime)) / 1000000.0;
         double rpy_rate[3];
         rpy_rate[0] = rpy[0] / elapsed_time;

pronto_biped_core/src/yawlock_common.cpp

@@ -1,5 +1,6 @@
 #include "pronto_biped_core/yawlock_common.hpp"
-#include <eigen_utils/eigen_utils.hpp>
+#include <iostream>
+#include <pronto_core/rotations.hpp>
 
 // if transitioned to standing, capture yaw
 // there after, issue this yaw as a correction to
@@ -113,8 +114,8 @@ bool YawLock::getCorrection(const Eigen::Isometry3d& world_to_body,
 
   std::cout << "\n";
   if (yaw_slip_detect_){
-    Eigen::Vector3d l_foot_to_r_foot_rpy = eigen_utils::getEulerAngles(Eigen::Quaterniond(l_foot_to_r_foot.rotation()));
-    Eigen::Vector3d l_foot_to_r_foot_original_rpy = eigen_utils::getEulerAngles(Eigen::Quaterniond(l_foot_to_r_foot_original_.rotation()));
+    Eigen::Vector3d l_foot_to_r_foot_rpy = rotation::getEulerAngles(Eigen::Quaterniond(l_foot_to_r_foot.rotation()));
+    Eigen::Vector3d l_foot_to_r_foot_original_rpy = rotation::getEulerAngles(Eigen::Quaterniond(l_foot_to_r_foot_original_.rotation()));
     double yaw_diff_change = fabs(l_foot_to_r_foot_rpy(2) - l_foot_to_r_foot_original_rpy(2));
     std::cout <<  "YAWLOCK: left-right yaw angle: "
                << l_foot_to_r_foot_original_rpy(2) * 180.0 / M_PI
@@ -176,7 +177,7 @@ bool YawLock::getCorrection(const Eigen::Isometry3d& world_to_body,
 
   Eigen::Vector3d rpy_update;
 
-  rpy_update = eigen_utils::getEulerAngles(world_to_body_quat_correction);
+  rpy_update = rotation::getEulerAngles(world_to_body_quat_correction);
   std::cout << "YAWLOCK: " << rpy_update.transpose() << " output rpy [rad]"
             << std::endl;
   std::cout << "YAWLOCK: " << rpy_update.transpose() * 180.0 / M_PI

pronto_biped_ros/CMakeLists.txt

@@ -105,11 +105,10 @@ add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 # )
 
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS ${PROJECT_NAME} # ${PROJECT_NAME}_node
+        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 install(DIRECTORY include/${PROJECT_NAME}/
    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}

pronto_core/CMakeLists.txt

@@ -4,47 +4,45 @@ project(pronto_core)
 set(CMAKE_CXX_STANDARD 14)
 set(PRONTO_CORE_LIB ${PROJECT_NAME})
 
-#required by the lcm-based state estimator library
-find_package(catkin REQUIRED COMPONENTS stl_utils
-                                        eigen_utils)
+find_package(catkin REQUIRED)
 
 find_package(Eigen3 REQUIRED)
 
 include_directories(include ${EIGEN3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
 
 catkin_package(INCLUDE_DIRS include
-               LIBRARIES ${PRONTO_CORE_LIB}
-               CATKIN_DEPENDS eigen_utils stl_utils
+               LIBRARIES ${PRONTO_CORE_LIB}               
                DEPENDS EIGEN3)
 
-# this library contains only computation, no dependency on LCM
-add_library (${PRONTO_CORE_LIB} src/rbis.cpp
-                                src/ins_module.cpp
-                                src/scan_matcher_module.cpp
-                                src/rbis_update_interface.cpp
-                                src/state_est.cpp
-                                src/update_history.cpp
-                                src/visual_odometry_module.cpp
-                                src/lidar_odometry_module.cpp
-                                src/vicon_module.cpp
-                                src/indexed_meas_module.cpp
-                                src/init_message_module.cpp
-                                src/gps_module.cpp
-                                src/pose_meas_module.cpp)
+add_library(${PRONTO_CORE_LIB} src/rbis.cpp
+                               src/rotations.cpp
+                               src/rigidbody.cpp
+                               src/ins_module.cpp
+                               src/scan_matcher_module.cpp
+                               src/rbis_update_interface.cpp
+                               src/state_est.cpp
+                               src/update_history.cpp
+                               src/visual_odometry_module.cpp
+                               src/lidar_odometry_module.cpp
+                               src/vicon_module.cpp
+                               src/indexed_meas_module.cpp
+                               src/init_message_module.cpp
+                               src/gps_module.cpp
+                               src/pose_meas_module.cpp)
 
 target_link_libraries(${PRONTO_CORE_LIB} ${catkin_LIBRARIES})
 
-catkin_add_gtest(ins_module_ballistic test/imu_module_test.cpp)
-target_link_libraries(ins_module_ballistic ${catkin_LIBRARIES} ${PRONTO_CORE_LIB} pthread)
+if(CATKIN_ENABLE_TESTING)
+  catkin_add_gtest(ins_module_ballistic test/imu_module_test.cpp)
+  target_link_libraries(ins_module_ballistic ${catkin_LIBRARIES} ${PRONTO_CORE_LIB} pthread)
+endif()
 
-
-install(TARGETS ${TARGETS}
+install(TARGETS ${TARGETS} ${PRONTO_CORE_LIB}
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 
 install(DIRECTORY include/${PROJECT_NAME}/
         DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-        FILES_MATCHING PATTERN "*.hpp"
-        PATTERN ".svn" EXCLUDE)
+        FILES_MATCHING PATTERN "*.hpp")

pronto_core/include/pronto_core/rbis.hpp

@@ -1,16 +1,15 @@
-#ifndef __RBS_estimator_h__
-#define __RBS_estimator_h__
+#pragma once
 
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
-#include <eigen_utils/eigen_utils.hpp>
+#include "pronto_core/rigidbody.hpp"
 
 namespace pronto {
 
 /**
  * Rigid body state
  */
-class RBIS: public eigen_utils::RigidBodyState {
+class RBIS: public RigidBodyState {
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 public:
@@ -55,10 +54,10 @@ class RBIS: public eigen_utils::RigidBodyState {
   /**
    * copy everything from rigid body state and biases are set to 0
    */
-  RBIS(const eigen_utils::RigidBodyState & rigid_body_state) :
+  RBIS(const RigidBodyState & rigid_body_state) :
       RigidBodyState(rbis_num_states)
   {
-    this->vec.head(eigen_utils::RigidBodyState::basic_num_states) = rigid_body_state.vec;
+    this->vec.head(RigidBodyState::basic_num_states) = rigid_body_state.vec;
     this->utime = rigid_body_state.utime;
     this->quat = rigid_body_state.quat;
   }
@@ -257,4 +256,3 @@ void ekfSmoothingStep(const RBIS & next_state_pred,
                       RBIM & cur_cov);
 
 }
-#endif