Support enable stream and publish them in ros
This commit is contained in:
John Zhao
parent
f3d6fcdcb9
commit
f636ba3a86
|
|
@ -6,17 +6,40 @@
|
|||
|
||||
<arg name="left_topic" default="left" />
|
||||
<arg name="right_topic" default="right" />
|
||||
<arg name="left_rect_topic" default="left_rect" />
|
||||
<arg name="right_rect_topic" default="right_rect" />
|
||||
<arg name="disparity_topic" default="disparity" />
|
||||
<arg name="disparity_norm_topic" default="disparity_norm" />
|
||||
<arg name="points_topic" default="points" />
|
||||
<arg name="depth_topic" default="depth" />
|
||||
|
||||
<arg name="imu_topic" default="imu" />
|
||||
<arg name="temp_topic" default="temp" />
|
||||
|
||||
<arg name="base_frame_id" default="$(arg mynteye)_link" />
|
||||
<arg name="left_frame_id" default="$(arg mynteye)_left_frame" />
|
||||
<arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
|
||||
<arg name="left_rect_frame_id" default="$(arg mynteye)_left_rect_frame" />
|
||||
<arg name="right_rect_frame_id" default="$(arg mynteye)_right_rect_frame" />
|
||||
<arg name="disparity_frame_id" default="$(arg mynteye)_disparity_frame" />
|
||||
<arg name="disparity_norm_frame_id" default="$(arg mynteye)_disparity_norm_frame" />
|
||||
<arg name="points_frame_id" default="$(arg mynteye)_points_frame" />
|
||||
<arg name="depth_frame_id" default="$(arg mynteye)_depth_frame" />
|
||||
|
||||
<arg name="imu_frame_id" default="$(arg mynteye)_imu_frame" />
|
||||
<arg name="temp_frame_id" default="$(arg mynteye)_temp_frame" />
|
||||
|
||||
<arg name="gravity" default="9.8" />
|
||||
|
||||
<!-- stream toggles -->
|
||||
|
||||
<arg name="enable_left_rect" default="false" />
|
||||
<arg name="enable_right_rect" default="false" />
|
||||
<arg name="enable_disparity" default="false" />
|
||||
<arg name="enable_disparity_norm" default="false" />
|
||||
<arg name="enable_points" default="false" />
|
||||
<arg name="enable_depth" default="true" />
|
||||
|
||||
<!-- device options, -1 will not set the value -->
|
||||
|
||||
<!-- gain range: [0,48] -->
|
||||
|
|
@ -56,7 +79,7 @@
|
|||
<!-- <arg name="desired_brightness" default="192" /> -->
|
||||
|
||||
<!-- ir_control range: [0,160] -->
|
||||
<arg name="ir_control" default="-1" />
|
||||
<arg name="ir_control" default="80" />
|
||||
<!-- <arg name="ir_control" default="0" /> -->
|
||||
|
||||
<!-- hdr_mode, 0: 10-bit, 1: 12-bit -->
|
||||
|
|
@ -73,17 +96,40 @@
|
|||
|
||||
<param name="left_topic" value="$(arg left_topic)" />
|
||||
<param name="right_topic" value="$(arg right_topic)" />
|
||||
<param name="left_rect_topic" value="$(arg left_rect_topic)" />
|
||||
<param name="right_rect_topic" value="$(arg right_rect_topic)" />
|
||||
<param name="disparity_topic" value="$(arg disparity_topic)" />
|
||||
<param name="disparity_norm_topic" value="$(arg disparity_norm_topic)" />
|
||||
<param name="points_topic" value="$(arg points_topic)" />
|
||||
<param name="depth_topic" value="$(arg depth_topic)" />
|
||||
|
||||
<param name="imu_topic" value="$(arg imu_topic)" />
|
||||
<param name="temp_topic" value="$(arg temp_topic)" />
|
||||
|
||||
<param name="base_frame_id" value="$(arg base_frame_id)" />
|
||||
<param name="left_frame_id" value="$(arg left_frame_id)" />
|
||||
<param name="right_frame_id" value="$(arg right_frame_id)" />
|
||||
<param name="left_rect_frame_id" value="$(arg left_rect_frame_id)" />
|
||||
<param name="right_rect_frame_id" value="$(arg right_rect_frame_id)" />
|
||||
<param name="disparity_frame_id" value="$(arg disparity_frame_id)" />
|
||||
<param name="disparity_norm_frame_id" value="$(arg disparity_norm_frame_id)" />
|
||||
<param name="points_frame_id" value="$(arg points_frame_id)" />
|
||||
<param name="depth_frame_id" value="$(arg depth_frame_id)" />
|
||||
|
||||
<param name="imu_frame_id" value="$(arg imu_frame_id)" />
|
||||
<param name="temp_frame_id" value="$(arg temp_frame_id)" />
|
||||
|
||||
<param name="gravity" value="$(arg gravity)" />
|
||||
|
||||
<!-- stream toggles -->
|
||||
|
||||
<param name="enable_left_rect" value="$(arg enable_left_rect)" />
|
||||
<param name="enable_right_rect" value="$(arg enable_right_rect)" />
|
||||
<param name="enable_disparity" value="$(arg enable_disparity)" />
|
||||
<param name="enable_disparity_norm" value="$(arg enable_disparity_norm)" />
|
||||
<param name="enable_points" value="$(arg enable_points)" />
|
||||
<param name="enable_depth" value="$(arg enable_depth)" />
|
||||
|
||||
<!-- device options -->
|
||||
|
||||
<param name="gain" value="$(arg gain)" />
|
||||
|
|
|
|||
|
|
@ -6,6 +6,9 @@ Panels:
|
|||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Images1
|
||||
- /Images1/Rectified1
|
||||
- /Disparity1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 853
|
||||
- Class: rviz/Selection
|
||||
|
|
@ -48,15 +51,8 @@ Visualization Manager:
|
|||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_plugin_tutorials/Imu
|
||||
Color: 204; 51; 204
|
||||
Enabled: false
|
||||
History Length: 1
|
||||
Name: Imu
|
||||
Topic: /mynteye/imu
|
||||
Unreliable: false
|
||||
Value: false
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /mynteye/left
|
||||
|
|
@ -81,6 +77,115 @@ Visualization Manager:
|
|||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /mynteye/left_rect
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: LeftRect
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /mynteye/right_rect
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: RightRect
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: false
|
||||
Name: Rectified
|
||||
Enabled: true
|
||||
Name: Images
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /mynteye/disparity
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Disparity
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /mynteye/disparity_norm
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: DisparityNorm
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: false
|
||||
Name: Disparity
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /mynteye/depth
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Depth
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 10
|
||||
Min Value: -10
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud2
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: ""
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: Points
|
||||
Position Transformer: ""
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.00999999978
|
||||
Style: Flat Squares
|
||||
Topic: /mynteye/points
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz_plugin_tutorials/Imu
|
||||
Color: 204; 51; 204
|
||||
Enabled: false
|
||||
History Length: 1
|
||||
Name: Imu
|
||||
Topic: /mynteye/imu
|
||||
Unreliable: false
|
||||
Value: false
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
|
|
@ -127,6 +232,12 @@ Visualization Manager:
|
|||
Yaw: 0.785398006
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Depth:
|
||||
collapsed: false
|
||||
Disparity:
|
||||
collapsed: false
|
||||
DisparityNorm:
|
||||
collapsed: false
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1056
|
||||
|
|
@ -134,9 +245,13 @@ Window Geometry:
|
|||
Hide Right Dock: false
|
||||
Left:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bb0000011e0000024f00000191fb0000000a00520069006700680074030000040c0000011e0000025000000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
LeftRect:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc020000000ffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000008004c00650066007403000001bd000000860000024e00000190fb0000000a00520069006700680074030000040c000000860000025000000190fb00000010004c006500660074005200650063007402000001bd000002170000024e00000190fb00000012005200690067006800740052006500630074020000040c000002170000024f00000190fb0000000a0044006500700074006803000001bc000002170000024f00000190fb0000001200440069007300700061007200690074007902000001bd000002170000024e0000018ffb0000001a004400690073007000610072006900740079004e006f0072006d020000040c000002170000025100000190000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Right:
|
||||
collapsed: false
|
||||
RightRect:
|
||||
collapsed: false
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
|
|
|
|||
|
|
@ -4,7 +4,9 @@
|
|||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/image_encodings.h>
|
||||
#include <tf/tf.h>
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
|
||||
#include <mynt_eye_ros_wrapper/Temp.h>
|
||||
|
|
@ -63,10 +65,21 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
|
||||
// node params
|
||||
|
||||
std::string left_topic = "left";
|
||||
std::string right_topic = "right";
|
||||
private_nh_.getParam("left_topic", left_topic);
|
||||
private_nh_.getParam("right_topic", right_topic);
|
||||
std::map<Stream, std::string> stream_names{
|
||||
{Stream::LEFT, "left"},
|
||||
{Stream::RIGHT, "right"},
|
||||
{Stream::LEFT_RECTIFIED, "left_rect"},
|
||||
{Stream::RIGHT_RECTIFIED, "right_rect"},
|
||||
{Stream::DISPARITY, "disparity"},
|
||||
{Stream::DISPARITY_NORMALIZED, "disparity_norm"},
|
||||
{Stream::DEPTH, "depth"},
|
||||
{Stream::POINTS, "points"}};
|
||||
|
||||
std::map<Stream, std::string> stream_topics{};
|
||||
for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
|
||||
stream_topics[it->first] = it->second;
|
||||
private_nh_.getParam(it->second + "_topic", stream_topics[it->first]);
|
||||
}
|
||||
|
||||
std::string imu_topic = "imu";
|
||||
std::string temp_topic = "temp";
|
||||
|
|
@ -76,10 +89,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
base_frame_id_ = "camera_link";
|
||||
private_nh_.getParam("base_frame_id", base_frame_id_);
|
||||
|
||||
camera_frame_ids_[Stream::LEFT] = "camera_left_frame";
|
||||
camera_frame_ids_[Stream::RIGHT] = "camera_right_frame";
|
||||
private_nh_.getParam("left_frame_id", camera_frame_ids_[Stream::LEFT]);
|
||||
private_nh_.getParam("right_frame_id", camera_frame_ids_[Stream::RIGHT]);
|
||||
for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
|
||||
frame_ids_[it->first] = "camera_" + it->second + "_frame";
|
||||
private_nh_.getParam(it->second + "_frame_id", frame_ids_[it->first]);
|
||||
}
|
||||
|
||||
imu_frame_id_ = "camera_imu_frame";
|
||||
temp_frame_id_ = "camera_temp_frame";
|
||||
|
|
@ -91,55 +104,79 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
|
||||
// device options
|
||||
|
||||
std::map<std::string, Option> option_map = {
|
||||
{"gain", Option::GAIN},
|
||||
{"brightness", Option::BRIGHTNESS},
|
||||
{"contrast", Option::CONTRAST},
|
||||
{"frame_rate", Option::FRAME_RATE},
|
||||
{"imu_frequency", Option::IMU_FREQUENCY},
|
||||
{"exposure_mode", Option::EXPOSURE_MODE},
|
||||
{"max_gain", Option::MAX_GAIN},
|
||||
{"max_exposure_time", Option::MAX_EXPOSURE_TIME},
|
||||
{"desired_brightness", Option::DESIRED_BRIGHTNESS},
|
||||
{"ir_control", Option::IR_CONTROL},
|
||||
{"hdr_mode", Option::HDR_MODE},
|
||||
std::map<Option, std::string> option_names = {
|
||||
{Option::GAIN, "gain"},
|
||||
{Option::BRIGHTNESS, "brightness"},
|
||||
{Option::CONTRAST, "contrast"},
|
||||
{Option::FRAME_RATE, "frame_rate"},
|
||||
{Option::IMU_FREQUENCY, "imu_frequency"},
|
||||
{Option::EXPOSURE_MODE, "exposure_mode"},
|
||||
{Option::MAX_GAIN, "max_gain"},
|
||||
{Option::MAX_EXPOSURE_TIME, "max_exposure_time"},
|
||||
{Option::DESIRED_BRIGHTNESS, "desired_brightness"},
|
||||
{Option::IR_CONTROL, "ir_control"},
|
||||
{Option::HDR_MODE, "hdr_mode"},
|
||||
};
|
||||
int value;
|
||||
for (auto &&it = option_map.begin(); it != option_map.end(); it++) {
|
||||
value = -1;
|
||||
private_nh_.getParam(it->first, value);
|
||||
for (auto &&it = option_names.begin(); it != option_names.end(); ++it) {
|
||||
if (!api_->Supports(it->first))
|
||||
continue;
|
||||
int value = -1;
|
||||
private_nh_.getParam(it->second, value);
|
||||
if (value != -1) {
|
||||
NODELET_INFO_STREAM("Set " << it->first << " to " << value);
|
||||
api_->SetOptionValue(it->second, value);
|
||||
NODELET_INFO_STREAM("Set " << it->second << " to " << value);
|
||||
api_->SetOptionValue(it->first, value);
|
||||
}
|
||||
NODELET_INFO_STREAM(
|
||||
it->second << ": " << api_->GetOptionValue(it->second));
|
||||
NODELET_INFO_STREAM(it->first << ": " << api_->GetOptionValue(it->first));
|
||||
}
|
||||
|
||||
// image publishers
|
||||
// publishers
|
||||
|
||||
image_transport::ImageTransport it_mynteye(nh_);
|
||||
|
||||
camera_encodings_[Stream::LEFT] = enc::MONO8;
|
||||
camera_publishers_[Stream::LEFT] =
|
||||
it_mynteye.advertiseCamera(left_topic, 1);
|
||||
NODELET_INFO_STREAM("Advertized on topic " << left_topic);
|
||||
for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
|
||||
auto &&topic = stream_topics[it->first];
|
||||
if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { // camera
|
||||
camera_publishers_[it->first] = it_mynteye.advertiseCamera(topic, 1);
|
||||
} else if (it->first == Stream::POINTS) { // pointcloud
|
||||
points_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(topic, 1);
|
||||
} else { // image
|
||||
image_publishers_[it->first] = it_mynteye.advertise(topic, 1);
|
||||
}
|
||||
NODELET_INFO_STREAM("Advertized on topic " << topic);
|
||||
}
|
||||
|
||||
camera_encodings_[Stream::RIGHT] = enc::MONO8;
|
||||
camera_publishers_[Stream::RIGHT] =
|
||||
it_mynteye.advertiseCamera(right_topic, 1);
|
||||
NODELET_INFO_STREAM("Advertized on topic " << right_topic);
|
||||
camera_encodings_ = {{Stream::LEFT, enc::MONO8},
|
||||
{Stream::RIGHT, enc::MONO8}};
|
||||
|
||||
// imu publisher
|
||||
image_encodings_ = {{Stream::LEFT_RECTIFIED, enc::MONO8},
|
||||
{Stream::RIGHT_RECTIFIED, enc::MONO8},
|
||||
{Stream::DISPARITY, enc::MONO8}, // float
|
||||
{Stream::DISPARITY_NORMALIZED, enc::MONO8},
|
||||
{Stream::DEPTH, enc::MONO16}};
|
||||
|
||||
pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 1);
|
||||
NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
|
||||
|
||||
// temp publisher
|
||||
|
||||
pub_temp_ = nh_.advertise<mynt_eye_ros_wrapper::Temp>(temp_topic, 1);
|
||||
NODELET_INFO_STREAM("Advertized on topic " << temp_topic);
|
||||
|
||||
// stream toggles
|
||||
|
||||
for (auto &&it = stream_names.begin(); it != stream_names.end(); ++it) {
|
||||
if (it->first == Stream::LEFT || it->first == Stream::RIGHT) { // camera
|
||||
continue;
|
||||
} else { // image, pointcloud
|
||||
if (!api_->Supports(it->first))
|
||||
continue;
|
||||
bool enabled = false;
|
||||
private_nh_.getParam("enable_" + it->second, enabled);
|
||||
if (enabled) {
|
||||
api_->EnableStreamData(it->first);
|
||||
NODELET_INFO_STREAM("Enable stream data of " << it->first);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
publishStaticTransforms();
|
||||
publishTopics();
|
||||
}
|
||||
|
|
@ -151,7 +188,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
geometry_msgs::TransformStamped b2l_msg;
|
||||
b2l_msg.header.stamp = tf_stamp;
|
||||
b2l_msg.header.frame_id = base_frame_id_;
|
||||
b2l_msg.child_frame_id = camera_frame_ids_[Stream::LEFT];
|
||||
b2l_msg.child_frame_id = frame_ids_[Stream::LEFT];
|
||||
b2l_msg.transform.translation.x = 0;
|
||||
b2l_msg.transform.translation.y = 0;
|
||||
b2l_msg.transform.translation.z = 0;
|
||||
|
|
@ -162,17 +199,24 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
static_tf_broadcaster_.sendTransform(b2l_msg);
|
||||
|
||||
// Transform base frame to right frame
|
||||
auto &&b2r_ex = api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT);
|
||||
tf::Quaternion b2r_q;
|
||||
tf::Matrix3x3 b2r_r(
|
||||
b2r_ex.rotation[0][0], b2r_ex.rotation[0][1], b2r_ex.rotation[0][2],
|
||||
b2r_ex.rotation[1][0], b2r_ex.rotation[1][1], b2r_ex.rotation[1][2],
|
||||
b2r_ex.rotation[2][0], b2r_ex.rotation[2][1], b2r_ex.rotation[2][2]);
|
||||
b2r_r.getRotation(b2r_q);
|
||||
geometry_msgs::TransformStamped b2r_msg;
|
||||
b2r_msg.header.stamp = tf_stamp;
|
||||
b2r_msg.header.frame_id = base_frame_id_;
|
||||
b2r_msg.child_frame_id = camera_frame_ids_[Stream::RIGHT];
|
||||
b2r_msg.transform.translation.x = 0;
|
||||
b2r_msg.transform.translation.y = 0;
|
||||
b2r_msg.transform.translation.z = 0;
|
||||
b2r_msg.transform.rotation.x = 0;
|
||||
b2r_msg.transform.rotation.y = 0;
|
||||
b2r_msg.transform.rotation.z = 0;
|
||||
b2r_msg.transform.rotation.w = 1;
|
||||
b2r_msg.child_frame_id = frame_ids_[Stream::RIGHT];
|
||||
b2r_msg.transform.translation.x = b2r_ex.translation[0];
|
||||
b2r_msg.transform.translation.y = b2r_ex.translation[1];
|
||||
b2r_msg.transform.translation.z = b2r_ex.translation[2];
|
||||
b2r_msg.transform.rotation.x = b2r_q.getX();
|
||||
b2r_msg.transform.rotation.y = b2r_q.getY();
|
||||
b2r_msg.transform.rotation.z = b2r_q.getZ();
|
||||
b2r_msg.transform.rotation.w = b2r_q.getW();
|
||||
static_tf_broadcaster_.sendTransform(b2r_msg);
|
||||
|
||||
// Transform base frame to imu frame
|
||||
|
|
@ -233,6 +277,19 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
<< ", exposure_time: " << data.img->exposure_time);
|
||||
});
|
||||
|
||||
std::vector<Stream> image_streams{
|
||||
Stream::LEFT_RECTIFIED, Stream::RIGHT_RECTIFIED, Stream::DISPARITY,
|
||||
Stream::DISPARITY_NORMALIZED, Stream::DEPTH};
|
||||
|
||||
for (auto &&stream : image_streams) {
|
||||
api_->SetStreamCallback(
|
||||
stream, [this, stream](const api::StreamData &data) {
|
||||
static std::size_t count = 0;
|
||||
++count;
|
||||
publishImage(stream, data, count, ros::Time::now());
|
||||
});
|
||||
}
|
||||
|
||||
api_->SetMotionCallback([this](const api::MotionData &data) {
|
||||
static double ros_time_beg = ros::Time::now().toSec();
|
||||
static double imu_time_beg = data.imu->timestamp;
|
||||
|
|
@ -277,15 +334,39 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
std_msgs::Header header;
|
||||
header.seq = seq;
|
||||
header.stamp = stamp;
|
||||
header.frame_id = camera_frame_ids_[stream];
|
||||
header.frame_id = frame_ids_[stream];
|
||||
cv::Mat img = data.frame;
|
||||
if (stream == Stream::DISPARITY) { // 32FC1 > 8UC1 = MONO8
|
||||
img.convertTo(img, CV_8UC1);
|
||||
}
|
||||
auto &&msg =
|
||||
cv_bridge::CvImage(header, camera_encodings_[stream], data.frame)
|
||||
.toImageMsg();
|
||||
cv_bridge::CvImage(header, camera_encodings_[stream], img).toImageMsg();
|
||||
auto &&info = getCameraInfo(stream);
|
||||
info->header.stamp = msg->header.stamp;
|
||||
camera_publishers_[stream].publish(msg, info);
|
||||
}
|
||||
|
||||
void publishImage(
|
||||
const Stream &stream, const api::StreamData &data, std::uint32_t seq,
|
||||
ros::Time stamp) {
|
||||
if (image_publishers_[stream].getNumSubscribers() == 0)
|
||||
return;
|
||||
std_msgs::Header header;
|
||||
header.seq = seq;
|
||||
header.stamp = stamp;
|
||||
header.frame_id = frame_ids_[stream];
|
||||
cv::Mat img = data.frame;
|
||||
if (stream == Stream::DISPARITY) { // 32FC1 > 8UC1 = MONO8
|
||||
img.convertTo(img, CV_8UC1);
|
||||
}
|
||||
auto &&msg =
|
||||
cv_bridge::CvImage(header, image_encodings_[stream], img).toImageMsg();
|
||||
image_publishers_[stream].publish(msg);
|
||||
}
|
||||
|
||||
// void publishPoints(
|
||||
// )
|
||||
|
||||
void publishImu(
|
||||
const api::MotionData &data, std::uint32_t seq, ros::Time stamp) {
|
||||
if (pub_imu_.getNumSubscribers() == 0)
|
||||
|
|
@ -394,7 +475,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
|
||||
auto &&in = api_->GetIntrinsics(stream);
|
||||
|
||||
camera_info->header.frame_id = camera_frame_ids_[stream];
|
||||
camera_info->header.frame_id = frame_ids_[stream];
|
||||
camera_info->width = in.width;
|
||||
camera_info->height = in.height;
|
||||
|
||||
|
|
@ -456,10 +537,19 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle private_nh_;
|
||||
|
||||
// camera: LEFT, RIGHT
|
||||
std::map<Stream, image_transport::CameraPublisher> camera_publishers_;
|
||||
std::map<Stream, sensor_msgs::CameraInfoPtr> camera_info_ptrs_;
|
||||
std::map<Stream, std::string> camera_encodings_;
|
||||
|
||||
// image: LEFT_RECTIFIED, RIGHT_RECTIFIED, DISPARITY, DISPARITY_NORMALIZED,
|
||||
// DEPTH
|
||||
std::map<Stream, image_transport::Publisher> image_publishers_;
|
||||
std::map<Stream, std::string> image_encodings_;
|
||||
|
||||
// pointcloud: POINTS
|
||||
ros::Publisher points_publisher_;
|
||||
|
||||
ros::Publisher pub_imu_;
|
||||
ros::Publisher pub_temp_;
|
||||
|
||||
|
|
@ -470,7 +560,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
|
|||
std::string base_frame_id_;
|
||||
std::string imu_frame_id_;
|
||||
std::string temp_frame_id_;
|
||||
std::map<Stream, std::string> camera_frame_ids_;
|
||||
std::map<Stream, std::string> frame_ids_;
|
||||
|
||||
double gravity_;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user