diff --git a/CMakeLists.txt b/CMakeLists.txt index 21a285f..3725fae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,19 +53,37 @@ set(DEPENDENCIES "nav_msgs" ) ament_target_dependencies(${EXECUTABLE_NAME} ${DEPENDENCIES}) - ## Declare a cpp library -add_library(${PROJECT_NAME} src/CLaserOdometry2D.cpp) +add_library(${PROJECT_NAME} SHARED src/CLaserOdometry2D.cpp) ## Declare a cpp executable ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES}) -install(TARGETS ${EXECUTABLE_NAME} - DESTINATION lib/${PROJECT_NAME}) +install(TARGETS ${EXECUTABLE_NAME} ${PROJECT_NAME} + DESTINATION lib) + +install(TARGETS ${PROJECT_NAME} + DESTINATION ../sha32_driver/lib) install( DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -ament_package() +install(DIRECTORY + include + DESTINATION . +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_include_directories(include) +ament_export_targets(export_${PROJECT_NAME}) + +ament_package() diff --git a/src/CLaserOdometry2D.cpp b/src/CLaserOdometry2D.cpp index f6f70a5..83330bd 100644 --- a/src/CLaserOdometry2D.cpp +++ b/src/CLaserOdometry2D.cpp @@ -251,8 +251,8 @@ bool CLaserOdometry2D::odometryCalculation(const sensor_msgs::msg::LaserScan& sc auto m_runtime = get_clock()->now() - start; - RCLCPP_INFO(get_logger(), "[rf2o] execution time (ms): %f", - m_runtime.seconds()*double(1000)); + // RCLCPP_INFO(get_logger(), "[rf2o] execution time (ms): %f", + // m_runtime.seconds()*double(1000)); //Update poses PoseUpdate(); @@ -812,7 +812,7 @@ bool CLaserOdometry2D::filterLevelSolution() Eigen::SelfAdjointEigenSolver eigensolver(cov_odo); if (eigensolver.info() != Eigen::Success) { - RCLCPP_WARN(get_logger(), "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated"); + // RCLCPP_WARN(get_logger(), "[rf2o] ERROR: Eigensolver couldn't find a solution. Pose is not updated"); return false; } @@ -935,18 +935,18 @@ void CLaserOdometry2D::PoseUpdate() kai_loc_old_(1) = -kai_abs_(0)*std::sin(phi) + kai_abs_(1)*std::cos(phi); kai_loc_old_(2) = kai_abs_(2); - RCLCPP_INFO(get_logger(), "[rf2o] LASERodom = [%f %f %f]", - laser_pose_.translation()(0), - laser_pose_.translation()(1), - rf2o::getYaw(laser_pose_.rotation())); + // RCLCPP_INFO(get_logger(), "[rf2o] LASERodom = [%f %f %f]", + // laser_pose_.translation()(0), + // laser_pose_.translation()(1), + // rf2o::getYaw(laser_pose_.rotation())); //Compose Transformations robot_pose_ = laser_pose_ * laser_pose_on_robot_inv_; - RCLCPP_INFO(get_logger(), "BASEodom = [%f %f %f]", - robot_pose_.translation()(0), - robot_pose_.translation()(1), - rf2o::getYaw(robot_pose_.rotation())); + // RCLCPP_INFO(get_logger(), "BASEodom = [%f %f %f]", + // robot_pose_.translation()(0), + // robot_pose_.translation()(1), + // rf2o::getYaw(robot_pose_.rotation())); // Estimate linear/angular speeds (mandatory for base_local_planner) // last_scan -> the last scan received