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_;