27 #include "geometry_msgs/TransformStamped.h" 33 nh_private_(nh_private),
54 std::string world_frame;
58 if (world_frame ==
"ned") {
60 }
else if (world_frame ==
"nwu"){
62 }
else if (world_frame ==
"enu"){
65 ROS_ERROR(
"The parameter world_frame was set to invalid value '%s'.", world_frame.c_str());
66 ROS_ERROR(
"Valid values are 'enu', 'ned' and 'nwu'. Setting to 'enu'.");
81 ROS_INFO(
"Using dt computed from message headers");
137 boost::mutex::scoped_lock lock(
mutex_);
139 const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
140 const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
142 ros::Time time = imu_msg_raw->header.stamp;
147 geometry_msgs::Quaternion init_q;
150 ROS_WARN_THROTTLE(5.0,
"The IMU seems to be in free fall, cannot determine gravity direction!");
158 ROS_INFO(
"First IMU message received.");
175 " The filter will not update the orientation.");
182 ang_vel.x, ang_vel.y, ang_vel.z,
183 lin_acc.x, lin_acc.y, lin_acc.z,
192 const ImuMsg::ConstPtr& imu_msg_raw,
193 const MagMsg::ConstPtr& mag_msg)
195 boost::mutex::scoped_lock lock(
mutex_);
197 const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
198 const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
199 const geometry_msgs::Vector3& mag_fld = mag_msg->magnetic_field;
201 ros::Time time = imu_msg_raw->header.stamp;
205 geometry_msgs::Vector3 mag_compensated;
206 mag_compensated.x = mag_fld.x -
mag_bias_.x;
207 mag_compensated.y = mag_fld.y -
mag_bias_.y;
208 mag_compensated.z = mag_fld.z -
mag_bias_.z;
217 if(!std::isfinite(mag_fld.x) || !std::isfinite(mag_fld.y) || !std::isfinite(mag_fld.z))
222 geometry_msgs::Quaternion init_q;
225 ROS_WARN_THROTTLE(5.0,
"The IMU seems to be in free fall or close to magnetic north pole, cannot determine gravity direction!");
233 ROS_INFO(
"First pair of IMU and magnetometer messages received.");
250 " The filter will not update the orientation.");
257 ang_vel.x, ang_vel.y, ang_vel.z,
258 lin_acc.x, lin_acc.y, lin_acc.z,
259 mag_compensated.x, mag_compensated.y, mag_compensated.z,
268 geometry_msgs::Quaternion orientation;
281 geometry_msgs::TransformStamped transform;
282 transform.header.stamp = imu_msg_raw->header.stamp;
287 transform.transform.rotation.w = q0;
288 transform.transform.rotation.x = -q1;
289 transform.transform.rotation.y = -q2;
290 transform.transform.rotation.z = -q3;
295 transform.transform.rotation.w = q0;
296 transform.transform.rotation.x = q1;
297 transform.transform.rotation.y = q2;
298 transform.transform.rotation.z = q3;
311 boost::make_shared<ImuMsg>(*imu_msg_raw);
313 imu_msg->orientation.w = q0;
314 imu_msg->orientation.x = q1;
315 imu_msg->orientation.y = q2;
316 imu_msg->orientation.z = q3;
319 imu_msg->orientation_covariance[1] = 0.0;
320 imu_msg->orientation_covariance[2] = 0.0;
321 imu_msg->orientation_covariance[3] = 0.0;
323 imu_msg->orientation_covariance[5] = 0.0;
324 imu_msg->orientation_covariance[6] = 0.0;
325 imu_msg->orientation_covariance[7] = 0.0;
332 geometry_msgs::Vector3Stamped rpy;
335 rpy.header = imu_msg_raw->header;
341 float roll,
float pitch,
float yaw)
343 geometry_msgs::Vector3Stamped rpy;
345 rpy.vector.y = pitch;
347 rpy.header.stamp = t;
356 boost::mutex::scoped_lock lock(
mutex_);
361 ROS_INFO(
"Imu filter gain set to %f", gain);
362 ROS_INFO(
"Gyro drift bias set to %f", zeta);
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
#define ROS_WARN_STREAM_THROTTLE(period, args)
message_filters::Subscriber< ImuMsg > ImuSubscriber
void publish(const boost::shared_ptr< M > &message) const
message_filters::sync_policies::ApproximateTime< ImuMsg, MagMsg > SyncPolicy
bool publish_debug_topics_
boost::shared_ptr< Synchronizer > sync_
double orientation_variance_
void publishRawMsg(const ros::Time &t, float roll, float pitch, float yaw)
void imuMagCallback(const ImuMsg::ConstPtr &imu_msg_raw, const MagMsg::ConstPtr &mav_msg)
void setOrientation(double q0, double q1, double q2, double q3)
ros::Publisher rpy_raw_debug_publisher_
void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
imu_filter_madgwick::ImuFilterMadgwickConfig FilterConfig
message_filters::Synchronizer< SyncPolicy > Synchronizer
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
message_filters::Subscriber< MagMsg > MagSubscriber
void checkTopicsTimerCallback(const ros::TimerEvent &)
void getOrientation(double &q0, double &q1, double &q2, double &q3)
boost::shared_ptr< FilterConfigServer > config_server_
ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private)
void setAlgorithmGain(double gain)
ros::NodeHandle nh_private_
boost::shared_ptr< ImuSubscriber > imu_subscriber_
#define ROS_WARN_THROTTLE(period,...)
void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt)
ros::Publisher imu_publisher_
dynamic_reconfigure::Server< FilterConfig > FilterConfigServer
void setWorldFrame(WorldFrame::WorldFrame frame)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishTransform(const ImuMsg::ConstPtr &imu_msg_raw)
#define ROS_WARN_STREAM(args)
ros::Timer check_topics_timer_
static bool computeOrientation(WorldFrame::WorldFrame frame, geometry_msgs::Vector3 acceleration, geometry_msgs::Vector3 magneticField, geometry_msgs::Quaternion &orientation)
void setDriftBiasGain(double zeta)
void reconfigCallback(FilterConfig &config, uint32_t level)
bool getParam(const std::string &key, std::string &s) const
void publishFilteredMsg(const ImuMsg::ConstPtr &imu_msg_raw)
WorldFrame::WorldFrame world_frame_
tf2_ros::TransformBroadcaster tf_broadcaster_
void imuCallback(const ImuMsg::ConstPtr &imu_msg_raw)
boost::shared_ptr< MagSubscriber > mag_subscriber_
ros::Publisher rpy_filtered_debug_publisher_
geometry_msgs::Vector3 mag_bias_