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