Skip to content
Snippets Groups Projects
Commit f2692e72 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Rename

parent 85ae44df
No related branches found
No related tags found
2 merge requests!5After cmake and const refactor,!3new release
...@@ -22,7 +22,7 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -22,7 +22,7 @@ find_package(catkin REQUIRED COMPONENTS
# find_package(Ceres REQUIRED) # find_package(Ceres REQUIRED)
# find_package(Eigen3 REQUIRED) # find_package(Eigen3 REQUIRED)
find_package(wolf 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 ## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed ## modules and global scripts declared therein get installed
...@@ -125,7 +125,7 @@ include_directories( ...@@ -125,7 +125,7 @@ include_directories(
include include
${EIGEN_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS}
${wolf_INCLUDE_DIRS} ${wolf_INCLUDE_DIRS}
${wolfIMU_INCLUDE_DIRS} ${wolfimu_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS}
) )
...@@ -134,7 +134,7 @@ include_directories( ...@@ -134,7 +134,7 @@ include_directories(
# add_library(${PROJECT_NAME} # add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/wolf_ros.cpp # 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 ## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries ## 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) ...@@ -158,9 +158,9 @@ add_library(wolf_subscriber_IMU src/wolf_subscriber_imu.cpp)
#add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg) #add_dependencies(${PROJECT_NAME}_visualizer ${PROJECT_NAME}_gencfg)
## Specify libraries to link a library or executable target against ## Specify libraries to link a library or executable target against
target_link_libraries(wolf_subscriber_IMU target_link_libraries(subscriber_imu
${wolf_LIBRARIES} ${wolf_LIBRARIES}
${wolfIMU_LIBRARIES} ${wolfimu_LIBRARIES}
${sensor_msgs_LIBRARIES} ${sensor_msgs_LIBRARIES}
) )
......
...@@ -5,8 +5,8 @@ ...@@ -5,8 +5,8 @@
#include <core/common/wolf.h> #include <core/common/wolf.h>
#include <core/problem/problem.h> #include <core/problem/problem.h>
#include <core/utils/params_server.hpp> #include <core/utils/params_server.hpp>
#include <IMU/capture/capture_IMU.h> #include <imu/capture/capture_imu.h>
#include <IMU/sensor/sensor_IMU.h> #include <imu/sensor/sensor_imu.h>
/************************** /**************************
* ROS includes * * ROS includes *
...@@ -24,21 +24,21 @@ ...@@ -24,21 +24,21 @@
/************************** /**************************
* WOLF-ROS includes * * WOLF-ROS includes *
**************************/ **************************/
#include "wolf_subscriber.h" #include "subscriber.h"
#include "subscriber_factory.h" #include "subscriber_factory.h"
using namespace wolf; using namespace wolf;
class WolfSubscriberImu : public WolfSubscriber class SubscriberImu : public Subscriber
{ {
public: public:
// Constructor // Constructor
WolfSubscriberImu(const SensorBasePtr& sensor_ptr); SubscriberImu(const SensorBasePtr& sensor_ptr);
virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic); virtual void initSubscriber(ros::NodeHandle& nh, const std::string& topic);
void callback(const sensor_msgs::Imu::ConstPtr& msg); 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)
#include "../include/wolf_subscriber_imu.h" #include "../include/subscriber_imu.h"
using namespace wolf; using namespace wolf;
// Constructor // Constructor
WolfSubscriberImu::WolfSubscriberImu(const SensorBasePtr& sensor_ptr) : SubscriberImu::SubscriberImu(const SensorBasePtr& sensor_ptr) :
WolfSubscriber(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; Eigen::Vector6d data;
data << msg->linear_acceleration.x, data << msg->linear_acceleration.x,
msg->linear_acceleration.y, msg->linear_acceleration.y,
...@@ -30,19 +30,19 @@ void WolfSubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg) ...@@ -30,19 +30,19 @@ void WolfSubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
// if covariance not filled // if covariance not filled
if (cov.topLeftCorner<3,3>().isApprox(Eigen::Matrix3d::Zero(),1e-9)) 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)) 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_, sensor_ptr_,
data, data,
cov); cov);
new_capture->process(); 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);
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment