diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt
index e925dc6..79548f1 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt
@@ -1,7 +1,6 @@
cmake_minimum_required(VERSION 2.8.3)
project(mynt_eye_ros_wrapper)
-
get_filename_component(SDK_DIR "${PROJECT_SOURCE_DIR}/../../../.." ABSOLUTE)
if(NOT CMAKE_BUILD_TYPE)
@@ -26,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
cv_bridge
geometry_msgs
image_transport
+ message_runtime
nodelet
roscpp
sensor_msgs
@@ -42,6 +42,18 @@ checkPackage("sensor_msgs" "")
checkPackage("std_msgs" "")
checkPackage("tf" "")
+## messages
+
+add_message_files(
+ FILES
+ Temp.msg
+)
+
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+)
+
catkin_package(
CATKIN_DEPENDS cv_bridge geometry_msgs image_transport nodelet roscpp sensor_msgs std_msgs tf
)
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 ec071a1..f601210 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
@@ -7,11 +7,13 @@
+
+
@@ -72,11 +74,13 @@
+
+
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg b/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg
new file mode 100644
index 0000000..2072419
--- /dev/null
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg
@@ -0,0 +1,2 @@
+std_msgs/Header header
+float32 data
diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml b/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml
index 68c3326..4c7779d 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/package.xml
@@ -16,6 +16,7 @@
cv_bridge
geometry_msgs
image_transport
+ message_generation
nodelet
roscpp
sensor_msgs
@@ -34,6 +35,7 @@
cv_bridge
geometry_msgs
image_transport
+ message_runtime
nodelet
roscpp
sensor_msgs
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 60ceacf..04d9f63 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
@@ -7,6 +7,8 @@
#include
#include
+#include
+
#include
#define _USE_MATH_DEFINES
@@ -63,18 +65,22 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
std::string left_topic = "left";
std::string right_topic = "right";
std::string imu_topic = "imu";
+ std::string temp_topic = "temp";
private_nh_.getParam("left_topic", left_topic);
private_nh_.getParam("right_topic", right_topic);
private_nh_.getParam("imu_topic", imu_topic);
+ private_nh_.getParam("temp_topic", temp_topic);
base_frame_id_ = "camera_link";
left_frame_id_ = "camera_left_frame";
right_frame_id_ = "camera_right_frame";
imu_frame_id_ = "camera_imu_frame";
+ temp_frame_id_ = "camera_temp_frame";
private_nh_.getParam("base_frame_id", base_frame_id_);
private_nh_.getParam("left_frame_id", left_frame_id_);
private_nh_.getParam("right_frame_id", right_frame_id_);
private_nh_.getParam("imu_frame_id", imu_frame_id_);
+ private_nh_.getParam("temp_frame_id", temp_frame_id_);
gravity_ = 9.8;
private_nh_.getParam("gravity", gravity_);
@@ -120,6 +126,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
pub_imu_ = nh_.advertise(imu_topic, 1);
NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
+ // temp publisher
+
+ pub_temp_ = nh_.advertise(temp_topic, 1);
+ NODELET_INFO_STREAM("Advertized on topic " << temp_topic);
+
publishStaticTransforms();
publishTopics();
}
@@ -230,6 +241,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
++imu_count_;
publishImu(data, imu_count_, stamp);
+ publishTemp(data.imu->temperature, imu_count_, stamp);
NODELET_DEBUG_STREAM(
"Imu count: " << imu_count_ << ", frame_id: " << data.imu->frame_id
<< ", timestamp: " << data.imu->timestamp
@@ -314,6 +326,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
pub_imu_.publish(msg);
}
+ void publishTemp(float temperature, std::uint32_t seq, ros::Time stamp) {
+ mynt_eye_ros_wrapper::Temp msg;
+ msg.header.seq = seq;
+ msg.header.stamp = stamp;
+ msg.header.frame_id = temp_frame_id_;
+ msg.data = temperature;
+ pub_temp_.publish(msg);
+ }
+
private:
void initDevice() {
NODELET_INFO_STREAM("Detecting MYNT EYE devices");
@@ -360,6 +381,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
image_transport::Publisher pub_right_;
ros::Publisher pub_imu_;
+ ros::Publisher pub_temp_;
tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
@@ -369,6 +391,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
std::string left_frame_id_;
std::string right_frame_id_;
std::string imu_frame_id_;
+ std::string temp_frame_id_;
double gravity_;