diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp index d18fae814bb5defba4dea758e1a93e556bc681d5..5e0e6243898563d6562468ce43a1a66a5533e443 100644 --- a/src/subscriber_imu.cpp +++ b/src/subscriber_imu.cpp @@ -198,7 +198,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg) } // Publish IMU message without bias and gravity - if (pub_imu_corrected_.getNumSubscribers() != 0) + if (pub_imu_corrected_.getNumSubscribers() != 0 or true) { if (sensor_ptr_->getProblem()->getDim() == 3) { @@ -233,7 +233,7 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg) imu_corrected.linear_acceleration.z = data(2); imu_corrected.angular_velocity.x = data(3); imu_corrected.angular_velocity.y = data(4); - imu_corrected.angular_velocity.z = data(5) - bias(5); + imu_corrected.angular_velocity.z = data(5) - bias(2); pub_imu_corrected_.publish(imu_corrected); diff --git a/src/subscriber_imu_enableable.cpp b/src/subscriber_imu_enableable.cpp index 73422beb7ae3148222d72eaad51868fc7465f90f..4daa5895eb0adbb3baf21e94ffac707d85bb1fdc 100644 --- a/src/subscriber_imu_enableable.cpp +++ b/src/subscriber_imu_enableable.cpp @@ -62,7 +62,7 @@ SubscriberImuEnableable::SubscriberImuEnableable(const std::string& _unique_name void SubscriberImuEnableable::initialize(ros::NodeHandle& nh, const std::string& topic) { SubscriberImu::initialize(nh,topic); - sub_ = nh.subscribe(topic, 1, &SubscriberImuEnableable::callback, this); + sub_ = nh.subscribe(topic, 10, &SubscriberImuEnableable::callback, this); enable_sub_ = nh.subscribe(topic_enable_, 1, &SubscriberImuEnableable::enableCallback, this); if (lowpass_filter_)