tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat(ros): added run laserscan launch file for S1030 device

Browse Source
This commit is contained in:
Osenberg 2019-03-27 17:23:13 +08:00
parent 8968f19819
commit ff11fc0940
4 changed files with 200 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
wrappers/ros/src/mynt_eye_ros_wrapper/config/laserscan/s1030_laserscan.yaml Normal file
View File

@ -0,0 +1,4 @@
# is_laserscan
is_laserscan: true

157
wrappers/ros/src/mynt_eye_ros_wrapper/launch/laserscan/mynteye_s1030_laserscan.launch Normal file
View File

@ -0,0 +1,157 @@
<?xml version="1.0"?>
<launch>
<arg name="mynteye" default="mynteye" />
<!-- mutiple device -->
<arg name="is_multiple" default="false" />
<!-- serial_number -->
<!-- if is_mutiple is true, must set serial_number -->
<arg name="serial_number" default="" />
<!-- depth_type 0: MONO16, 1: TYPE_16UC1 -->
<arg name="depth_type" default="1" />
<!-- node params -->
<arg name="left_topic" default="left/image_raw" />
<arg name="right_topic" default="right/image_raw" />
<arg name="left_rect_topic" default="left_rect/image_rect" />
<arg name="right_rect_topic" default="right_rect/image_rect" />
<arg name="disparity_topic" default="disparity/image_raw" />
<arg name="disparity_norm_topic" default="disparity/image_norm" />
<arg name="depth_topic" default="depth/image_raw" />
<arg name="points_topic" default="points/data_raw" />
<arg name="left_mono_topic" default="left/image_mono" />
<arg name="right_mono_topic" default="right/image_mono" />
<arg name="imu_topic" default="imu/data_raw" />
<arg name="temperature_topic" default="temperature/data_raw" />
<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="temperature_frame_id" default="$(arg mynteye)_temperature_frame" />
<arg name="gravity" default="9.8" />
<arg name="mesh_file" default="S1030-0315.obj" />
<!-- Push down all topics/nodelets into "mynteye" namespace -->
<group ns="$(arg mynteye)">
<!-- mynteye_wrapper_node -->
<node name="mynteye_wrapper_node" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen" respawn="true" respawn_delay="5">
<!-- node params -->
<param name="is_multiple" value="$(arg is_multiple)" />
<param name="serial_number" value="$(arg serial_number)" />
<param name="depth_type" value="$(arg depth_type)" />
<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="mesh_file" value="$(arg mesh_file)" />
<param name="left_mono_topic" value="$(arg left_mono_topic)" />
<param name="right_mono_topic" value="$(arg right_mono_topic)" />
<param name="imu_topic" value="$(arg imu_topic)" />
<param name="temperature_topic" value="$(arg temperature_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="temperature_frame_id" value="$(arg temperature_frame_id)" />
<rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard.yaml" command="load" />
<rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard2.yaml" command="load" />
<rosparam file="$(find mynt_eye_ros_wrapper)/config/process/process_config.yaml" command="load" />
<rosparam file="$(find mynt_eye_ros_wrapper)/config/laserscan/s1030_laserscan.yaml" command="load" />
<param name="gravity" value="$(arg gravity)" />
</node>
<!-- disable compressed depth plugin for image topics -->
<group ns="$(arg left_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg left_rect_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg left_mono_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg right_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg right_mono_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg right_rect_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg disparity_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg disparity_norm_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
<group ns="$(arg depth_topic)">
<rosparam param="disable_pub_plugins">
- 'image_transport/compressedDepth'
</rosparam>
</group>
</group> <!-- mynteye -->
<node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan_loader" args="standalone depthimage_to_laserscan/DepthImageToLaserScanNodelet" output="screen">
<param name="scan_height" value="1" />
<param name="output_frame_id" value="depth_to_laserscan" />
<param name="range_min" value="0.45" />
<param name="range_max" value="10" />
<remap from="image" to="/mynteye/depth/image_raw" />
<remap from="camera_info" to="/mynteye/depth/camera_info" />
</node>
<node pkg="tf" type="static_transform_publisher" name="transform_coordinate" args="1 0 0 0 0 0 1 base_link depth_to_laserscan 100">
</node>
</launch>

5
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -9,6 +9,9 @@
<!-- if is_mutiple is true, must set serial_number -->
<arg name="serial_number" default="" />
<!-- depth_type 0: MONO16, 1: TYPE_16UC1 -->
<arg name="depth_type" default="0" />
<!-- node params -->
<arg name="left_topic" default="left/image_raw" />
@ -52,6 +55,8 @@
<param name="is_multiple" value="$(arg is_multiple)" />
<param name="serial_number" value="$(arg serial_number)" />
<param name="depth_type" value="$(arg depth_type)" />
<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)" />

23
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -324,6 +324,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
}
int depth_type = 0;
private_nh_.getParamCached("depth_type", depth_type);
if (model_ == Model::STANDARD2 || model_ == Model::STANDARD210A) {
camera_encodings_ = {{Stream::LEFT, enc::BGR8},
{Stream::RIGHT, enc::BGR8},
@ -334,6 +336,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
{Stream::DEPTH, enc::MONO16}};
}
if (model_ == Model::STANDARD) {
if (depth_type == 0) {
camera_encodings_ = {{Stream::LEFT, enc::MONO8},
{Stream::RIGHT, enc::MONO8},
{Stream::LEFT_RECTIFIED, enc::MONO8},
@ -341,6 +344,15 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
{Stream::DISPARITY, enc::MONO8}, // float
{Stream::DISPARITY_NORMALIZED, enc::MONO8},
{Stream::DEPTH, enc::MONO16}};
} else if (depth_type == 1) {
camera_encodings_ = {{Stream::LEFT, enc::MONO8},
{Stream::RIGHT, enc::MONO8},
{Stream::LEFT_RECTIFIED, enc::MONO8},
{Stream::RIGHT_RECTIFIED, enc::MONO8},
{Stream::DISPARITY, enc::MONO8}, // float
{Stream::DISPARITY_NORMALIZED, enc::MONO8},
{Stream::DEPTH, enc::TYPE_16UC1}};
}
}
pub_imu_ = nh_.advertise<sensor_msgs::Imu>(imu_topic, 100);
NODELET_INFO_STREAM("Advertized on topic " << imu_topic);
@ -1312,11 +1324,22 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
}
} else {
if (info_pair->left.distortion_model == "KANNALA_BRANDT") {
// compatible laserscan
bool is_laserscan = false;
private_nh_.getParamCached("is_laserscan", is_laserscan);
if (!is_laserscan) {
camera_info->distortion_model =
sensor_msgs::distortion_models::EQUIDISTANT;
for (size_t i; i < 4; i++) {
camera_info->D.push_back(info_pair->left.D[i]);
}
} else {
camera_info->distortion_model =
sensor_msgs::distortion_models::PLUMB_BOB;
for (size_t i; i < 4; i++) {
camera_info->D.push_back(0.0);
}
}
} else if (info_pair->left.distortion_model == "PINHOLE") {
camera_info->distortion_model =
sensor_msgs::distortion_models::PLUMB_BOB;