34 #include <std_msgs/Float64.h> 35 #include <std_msgs/Bool.h> 43 nh_private_(nh_private),
44 initialized_filter_(false)
46 ROS_INFO(
"Starting ComplementaryFilterROS");
76 sync_->registerCallback(
88 ROS_INFO(
"Destroying ComplementaryFilterROS");
95 bool do_bias_estimation;
97 bool do_adaptive_gain;
116 do_bias_estimation =
true;
120 do_adaptive_gain =
true;
122 double orientation_stddev;
124 orientation_stddev = 0.0;
132 ROS_WARN(
"Invalid gain_acc passed to ComplementaryFilter.");
136 ROS_WARN(
"Invalid gain_mag passed to ComplementaryFilter.");
138 if (do_bias_estimation)
141 ROS_WARN(
"Invalid bias_alpha passed to ComplementaryFilter.");
156 const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
157 const geometry_msgs::Vector3&
w = imu_msg_raw->angular_velocity;
158 const ros::Time& time = imu_msg_raw->header.stamp;
185 const MagMsg::ConstPtr& mag_msg)
187 const geometry_msgs::Vector3& a = imu_msg_raw->linear_acceleration;
188 const geometry_msgs::Vector3&
w = imu_msg_raw->angular_velocity;
189 const geometry_msgs::Vector3& m = mag_msg->magnetic_field;
190 const ros::Time& time = imu_msg_raw->header.stamp;
206 if (std::isnan(m.x) || std::isnan(m.y) || std::isnan(m.z))
209 filter_.
update(a.x, a.y, a.z, w.x, w.y, w.z, m.x, m.y, m.z, dt);
219 double q0,
double q1,
double q2,
double q3)
const 227 const sensor_msgs::Imu::ConstPtr& imu_msg_raw)
230 double q0, q1, q2, q3;
236 boost::make_shared<sensor_msgs::Imu>(*imu_msg_raw);
240 imu_msg->orientation_covariance[1] = 0.0;
241 imu_msg->orientation_covariance[2] = 0.0;
242 imu_msg->orientation_covariance[3] = 0.0;
244 imu_msg->orientation_covariance[5] = 0.0;
245 imu_msg->orientation_covariance[6] = 0.0;
246 imu_msg->orientation_covariance[7] = 0.0;
262 geometry_msgs::Vector3Stamped rpy;
263 rpy.header = imu_msg_raw->header;
267 M.
getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
273 std_msgs::Bool state_msg;
290 imu_msg_raw->header.stamp,
291 imu_msg_raw->header.frame_id,
298 imu_msg_raw->header.stamp,
300 imu_msg_raw->header.frame_id));
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
void setRotation(const Quaternion &q)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
TFSIMD_FORCE_INLINE const tfScalar & w() const
bool getParam(const std::string &key, std::string &s) const