diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index 6eeef94..0250024 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -13,6 +13,8 @@ + + @@ -76,6 +78,8 @@ + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 2076bee..bd7df79 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -9,6 +9,8 @@ #include +#define _USE_MATH_DEFINES +#include #include #include @@ -71,6 +73,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet { private_nh_.getParam("right_frame_id", right_frame_id_); private_nh_.getParam("imu_frame_id", imu_frame_id_); + gravity_ = 9.8; + private_nh_.getParam("gravity", gravity_); + // device options std::map option_map = { @@ -246,9 +251,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet { msg.header.frame_id = imu_frame_id_; // acceleration should be in m/s^2 (not in g's) - msg.linear_acceleration.x = data.imu->accel[0] * 9.8; - msg.linear_acceleration.y = data.imu->accel[1] * 9.8; - msg.linear_acceleration.z = data.imu->accel[2] * 9.8; + msg.linear_acceleration.x = data.imu->accel[0] * gravity_; + msg.linear_acceleration.y = data.imu->accel[1] * gravity_; + msg.linear_acceleration.z = data.imu->accel[2] * gravity_; msg.linear_acceleration_covariance[0] = 0; msg.linear_acceleration_covariance[1] = 0; @@ -263,9 +268,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet { msg.linear_acceleration_covariance[8] = 0; // velocity should be in rad/sec - msg.angular_velocity.x = data.imu->gyro[0] / 57.2956; - msg.angular_velocity.y = data.imu->gyro[1] / 57.2956; - msg.angular_velocity.z = data.imu->gyro[2] / 57.2956; + msg.angular_velocity.x = data.imu->gyro[0] * M_PI / 180; + msg.angular_velocity.y = data.imu->gyro[1] * M_PI / 180; + msg.angular_velocity.z = data.imu->gyro[2] * M_PI / 180; msg.angular_velocity_covariance[0] = 0; msg.angular_velocity_covariance[1] = 0; @@ -338,6 +343,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { std::string right_frame_id_; std::string imu_frame_id_; + double gravity_; + // device std::shared_ptr device_;