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

Merge branch 'develop' of http://gitlab.mynt.com/mynteye/mynt-eye-sdk-2 into develop

Browse Source
This commit is contained in:
TinyOh 2019-01-07 17:11:29 +08:00
commit d146cd92bc
4 changed files with 28 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt
View File

@ -57,11 +57,6 @@ checkPackage("tf" "")
## messages
add_message_files(
FILES
Temp.msg
)
add_service_files(
FILES
GetInfo.srv

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

@ -17,7 +17,7 @@
<arg name="right_mono_topic" default="right/image_mono" />
<arg name="imu_topic" default="imu/data_raw" />
<arg name="temp_topic" default="temp/data_raw" />
<arg name="temperature_topic" default="temperature/data_raw" />
<arg name="base_frame_id" default="$(arg mynteye)_link" />
<arg name="left_frame_id" default="$(arg mynteye)_left_frame" />
@ -29,7 +29,7 @@
<arg name="points_frame_id" default="$(arg mynteye)_points_frame" />
<arg name="depth_frame_id" default="$(arg mynteye)_depth_frame" />
<arg name="temp_frame_id" default="$(arg mynteye)_temp_frame" />
<arg name="temperature_frame_id" default="$(arg mynteye)_temperature_frame" />
<arg name="gravity" default="9.8" />
@ -226,7 +226,7 @@
<param name="right_mono_topic" value="$(arg right_mono_topic)" />
<param name="imu_topic" value="$(arg imu_topic)" />
<param name="temp_topic" value="$(arg temp_topic)" />
<param name="temperature_topic" value="$(arg temperature_topic)" />
<param name="base_frame_id" value="$(arg base_frame_id)" />
<param name="left_frame_id" value="$(arg left_frame_id)" />
@ -238,7 +238,7 @@
<param name="points_frame_id" value="$(arg points_frame_id)" />
<param name="depth_frame_id" value="$(arg depth_frame_id)" />
<param name="temp_frame_id" value="$(arg temp_frame_id)" />
<param name="temperature_frame_id" value="$(arg temperature_frame_id)" />
<param name="gravity" value="$(arg gravity)" />

2
wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg
View File

@ -1,2 +0,0 @@
std_msgs/Header header
float32 data

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

@ -18,6 +18,7 @@
#include <image_transport/image_transport.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Temperature.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf/tf.h>
@ -26,7 +27,6 @@
#include <opencv2/calib3d/calib3d.hpp>
#include <mynt_eye_ros_wrapper/GetInfo.h>
#include <mynt_eye_ros_wrapper/Temp.h>
#define _USE_MATH_DEFINES
#include <cmath>
@ -200,9 +200,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
std::string imu_topic = "imu";
std::string temp_topic = "temp";
std::string temperature_topic = "temperature";
private_nh_.getParam("imu_topic", imu_topic);
private_nh_.getParam("temp_topic", temp_topic);
private_nh_.getParam("temperature_topic", temperature_topic);
base_frame_id_ = "camera_link";
private_nh_.getParam("base_frame_id", base_frame_id_);
@ -213,9 +213,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
imu_frame_id_ = "camera_imu_frame";
temp_frame_id_ = "camera_temp_frame";
temperature_frame_id_ = "camera_temperature_frame";
private_nh_.getParam("imu_frame_id", imu_frame_id_);
private_nh_.getParam("temp_frame_id", temp_frame_id_);
private_nh_.getParam("temperature_frame_id", temperature_frame_id_);
gravity_ = 9.8;
private_nh_.getParam("gravity", gravity_);
@ -327,8 +327,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 100);
NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
pub_temp_ = nh_.advertise<mynt_eye_ros_wrapper::Temp>(temp_topic, 100);
NODELET_INFO_STREAM("Advertized on topic " << temp_topic);
pub_temperature_ = nh_.advertise<
sensor_msgs::Temperature>(temperature_topic, 100);
NODELET_INFO_STREAM("Advertized on topic " << temperature_topic);
// stream toggles
@ -576,14 +577,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
publishImuBySync(stamp);
} else {
publishImu(data, imu_count_, stamp);
publishTemp(data.imu->temperature, imu_count_, stamp);
publishTemperature(data.imu->temperature, imu_count_, stamp);
}
} else {
NODELET_WARN_STREAM("Motion data is empty");
}
} else {
publishImu(data, imu_count_, stamp);
publishTemp(data.imu->temperature, imu_count_, stamp);
publishTemperature(data.imu->temperature, imu_count_, stamp);
}
NODELET_DEBUG_STREAM(
"Imu count: " << imu_count_
@ -820,22 +821,24 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
pub_imu_.publish(msg);
publishTemp(imu_accel_->temperature, imu_sync_count_, stamp);
publishTemperature(imu_accel_->temperature, imu_sync_count_, stamp);
++imu_sync_count_;
imu_accel_ = nullptr;
imu_gyro_ = nullptr;
}
void publishTemp(float temperature, std::uint32_t seq, ros::Time stamp) {
if (pub_temp_.getNumSubscribers() == 0)
void publishTemperature(
float temperature, std::uint32_t seq, ros::Time stamp) {
if (pub_temperature_.getNumSubscribers() == 0)
return;
mynt_eye_ros_wrapper::Temp msg;
sensor_msgs::Temperature msg;
msg.header.seq = seq;
msg.header.stamp = stamp;
msg.header.frame_id = temp_frame_id_;
msg.data = temperature;
pub_temp_.publish(msg);
msg.header.frame_id = temperature_frame_id_;
msg.temperature = temperature;
msg.variance = 0;
pub_temperature_.publish(msg);
}
private:
@ -1085,9 +1088,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
l2r_msg.header.stamp = tf_stamp;
l2r_msg.header.frame_id = frame_ids_[Stream::LEFT];
l2r_msg.child_frame_id = frame_ids_[Stream::RIGHT];
l2r_msg.transform.translation.x = l2r_ex.translation[0];
l2r_msg.transform.translation.y = l2r_ex.translation[1];
l2r_msg.transform.translation.z = l2r_ex.translation[2];
l2r_msg.transform.translation.x = l2r_ex.translation[0] / 1000;
l2r_msg.transform.translation.y = l2r_ex.translation[1] / 1000;
l2r_msg.transform.translation.z = l2r_ex.translation[2] / 1000;
l2r_msg.transform.rotation.x = l2r_q.getX();
l2r_msg.transform.rotation.y = l2r_q.getY();
l2r_msg.transform.rotation.z = l2r_q.getZ();
@ -1234,7 +1237,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
ros::Publisher points_publisher_;
ros::Publisher pub_imu_;
ros::Publisher pub_temp_;
ros::Publisher pub_temperature_;
tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
@ -1244,7 +1247,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
std::string base_frame_id_;
std::string imu_frame_id_;
std::string temp_frame_id_;
std::string temperature_frame_id_;
std::map<Stream, std::string> frame_ids_;
double gravity_;