tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add gravity node param

Browse Source
This commit is contained in:
John Zhao 2018-04-14 16:47:05 +08:00
parent c3bac16750
commit b1f89ffbc2
2 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -13,6 +13,8 @@
<arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
<arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
<arg name="gravity" default="9.8" />
<!-- device options, -1 will not set the value -->
<!-- gain range: [0,48]-->
@ -76,6 +78,8 @@
<param name="right_frame_id" value="$(arg right_frame_id)" />
<param name="imu_frame_id" value="$(arg imu_frame_id)" />
<param name="gravity" value="$(arg gravity)" />
<!-- device options -->
<param name="gain" value="$(arg gain)" />

19
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -9,6 +9,8 @@
#include <glog/logging.h>
#define _USE_MATH_DEFINES
#include <cmath>
#include <map>
#include <string>
@ -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<std::string, Option> 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> device_;