diff --git a/CMakeLists.txt b/CMakeLists.txt index a3225f5c55ed75dcce283032cb18e10583ba31e0..b307bc503978262ac172cc2d5c3e9932e3cea1ba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,7 +22,7 @@ find_package(catkin REQUIRED COMPONENTS # find_package(Ceres REQUIRED) # find_package(Eigen3 REQUIRED) find_package(wolf REQUIRED) -find_package(wolfIMU REQUIRED) +find_package(wolfimu REQUIRED) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed @@ -125,7 +125,7 @@ include_directories( include ${EIGEN_INCLUDE_DIRS} ${wolf_INCLUDE_DIRS} - ${wolfIMU_INCLUDE_DIRS} + ${wolfimu_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ) @@ -134,7 +134,7 @@ include_directories( # add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/wolf_ros.cpp # ) -add_library(wolf_subscriber_IMU src/wolf_subscriber_imu.cpp) +add_library(subscriber_imu src/subscriber_imu.cpp) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries @@ -158,9 +158,9 @@ add_library(wolf_subscriber_IMU src/wolf_subscriber_imu.cpp) #add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg) ## Specify libraries to link a library or executable target against -target_link_libraries(wolf_subscriber_IMU +target_link_libraries(subscriber_imu ${wolf_LIBRARIES} - ${wolfIMU_LIBRARIES} + ${wolfimu_LIBRARIES} ${sensor_msgs_LIBRARIES} ) diff --git a/include/wolf_subscriber_imu.h b/include/subscriber_imu.h similarity index 67% rename from include/wolf_subscriber_imu.h rename to include/subscriber_imu.h index 015066328c77267179491cf10701cc5b5129372d..89c0ac51ca4c2e9b095552438c5f11fe4ff926f8 100644 --- a/include/wolf_subscriber_imu.h +++ b/include/subscriber_imu.h @@ -5,8 +5,8 @@ #include <core/common/wolf.h> #include <core/problem/problem.h> #include <core/utils/params_server.hpp> -#include <IMU/capture/capture_IMU.h> -#include <IMU/sensor/sensor_IMU.h> +#include <imu/capture/capture_imu.h> +#include <imu/sensor/sensor_imu.h> /************************** * ROS includes * @@ -24,21 +24,21 @@ /************************** * WOLF-ROS includes * **************************/ -#include "wolf_subscriber.h" +#include "subscriber.h" #include "subscriber_factory.h" using namespace wolf; -class WolfSubscriberImu : public WolfSubscriber +class SubscriberImu : public Subscriber { public: // Constructor - WolfSubscriberImu(const SensorBasePtr& sensor_ptr); + SubscriberImu(const SensorBasePtr& sensor_ptr); virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic); void callback(const sensor_msgs::Imu::ConstPtr& msg); - static std::shared_ptr<WolfSubscriber> create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr); + static std::shared_ptr<Subscriber> create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr); }; -WOLF_REGISTER_SUBSCRIBER(WolfSubscriberImu) +WOLF_REGISTER_SUBSCRIBER(SubscriberImu) diff --git a/src/wolf_subscriber_imu.cpp b/src/subscriber_imu.cpp similarity index 58% rename from src/wolf_subscriber_imu.cpp rename to src/subscriber_imu.cpp index 69c3ab10e57f871900523d9bda4fd312b14dcc57..a621a964476047c0b4f4172a2575e27970d5ce86 100644 --- a/src/wolf_subscriber_imu.cpp +++ b/src/subscriber_imu.cpp @@ -1,22 +1,22 @@ -#include "../include/wolf_subscriber_imu.h" +#include "../include/subscriber_imu.h" using namespace wolf; // Constructor -WolfSubscriberImu::WolfSubscriberImu(const SensorBasePtr& sensor_ptr) : - WolfSubscriber(sensor_ptr) +SubscriberImu::SubscriberImu(const SensorBasePtr& sensor_ptr) : + Subscriber(sensor_ptr) { - assert(std::dynamic_pointer_cast<SensorIMU>(sensor_ptr) != nullptr); + assert(std::dynamic_pointer_cast<SensorImu>(sensor_ptr) != nullptr); } -void WolfSubscriberImu::initSubscriber(ros::NodeHandle& nh, const std::string& topic) +void SubscriberImu::initSubscriber(ros::NodeHandle& nh, const std::string& topic) { - sub_ = nh.subscribe(topic, 1, &WolfSubscriberImu::callback, this); + sub_ = nh.subscribe(topic, 1, &SubscriberImu::callback, this); } -void WolfSubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg) +void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg) { - ROS_INFO("callback IMU!"); + ROS_INFO("callback Imu!"); Eigen::Vector6d data; data << msg->linear_acceleration.x, msg->linear_acceleration.y, @@ -30,19 +30,19 @@ void WolfSubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg) // if covariance not filled if (cov.topLeftCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9)) - cov.topLeftCorner<3,3>() = std::pow(std::static_pointer_cast<SensorIMU>(sensor_ptr_)->getAccelNoise(),2.0)*Eigen::Matrix3d::Identity(); + cov.topLeftCorner<3,3>() = std::pow(std::static_pointer_cast<SensorImu>(sensor_ptr_)->getAccelNoise(),2.0)*Eigen::Matrix3d::Identity(); if (cov.bottomRightCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9)) - cov.bottomRightCorner<3,3>() = std::pow(std::static_pointer_cast<SensorIMU>(sensor_ptr_)->getGyroNoise(),2.0)*Eigen::Matrix3d::Identity(); + cov.bottomRightCorner<3,3>() = std::pow(std::static_pointer_cast<SensorImu>(sensor_ptr_)->getGyroNoise(),2.0)*Eigen::Matrix3d::Identity(); - CaptureIMUPtr new_capture = std::make_shared<CaptureIMU>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), + CaptureImuPtr new_capture = std::make_shared<CaptureImu>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec), sensor_ptr_, data, cov); new_capture->process(); } -std::shared_ptr<WolfSubscriber> WolfSubscriberImu::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr) +std::shared_ptr<Subscriber> SubscriberImu::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr) { - return std::make_shared<WolfSubscriberImu>(_sensor_ptr); + return std::make_shared<SubscriberImu>(_sensor_ptr); }