diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt index cc73eec..ab949f9 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt @@ -57,11 +57,6 @@ checkPackage("tf" "") ## messages -add_message_files( - FILES - Temp.msg -) - add_service_files( FILES GetInfo.srv 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 b47507a..fcea5db 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -17,7 +17,7 @@ - + @@ -29,7 +29,7 @@ - + @@ -226,7 +226,7 @@ - + @@ -238,7 +238,7 @@ - + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg b/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg deleted file mode 100644 index 2072419..0000000 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/msg/Temp.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -float32 data 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 842ad11..5929cf9 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 @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -26,7 +27,6 @@ #include #include -#include #define _USE_MATH_DEFINES #include @@ -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(imu_topic, 100); NODELET_INFO_STREAM("Advertized on topic " << imu_topic); - pub_temp_ = nh_.advertise(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 frame_ids_; double gravity_;