apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added 3d example with zed, added bgra support on rgb/depth callback

Browse Source
This commit is contained in:
matlabbe 2019-01-28 16:37:35 -05:00
parent 21c80c9d82
commit ef0c565b7e
3 changed files with 36 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
launch/find_object_3d_kinect2.launch
View File

@ -16,17 +16,4 @@
<remap from="depth_registered/image_raw" to="/kinect2/$(arg resolution)/image_depth_rect"/>
<remap from="depth_registered/camera_info" to="/kinect2/$(arg resolution)/camera_info"/>
</node>
<!-- Example of tf synchronisation with the objectsStamped message -->
<node name="tf_example" pkg="find_object_2d" type="tf_example" output="screen">
<param name="map_frame_id" value="/map" type="string"/>
<param name="object_prefix" value="object" type="str"/>
</node>
<!-- fake some tf frames for the example /map -> /odom -> /base_link -> /kinect2_base_link -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0.1 0.0 0.3 0.0 0.0 0.0 /base_link /kinect2_link 100" />
<node pkg="tf" type="static_transform_publisher" name="odom_to_base_tf"
args="1.0 0.0 0.1 1.5707 0.0 0.0 /odom /base_link 100" />
<node pkg="tf" type="static_transform_publisher" name="map_to_odom_tf"
args="0.0 0.5 0.0 0.7853 0.0 0.0 /map /odom 100" />
</launch>

16
launch/find_object_3d_zed.launch Normal file
View File

@ -0,0 +1,16 @@
<launch>
<!-- Example finding 3D poses of the objects detected -->
<!-- $ roslaunch zed_wrapper zed.launch -->
<node name="find_object_3d" pkg="find_object_2d" type="find_object_2d" output="screen">
<param name="gui" value="true" type="bool"/>
<param name="settings_path" value="~/.ros/find_object_2d.ini" type="str"/>
<param name="subscribe_depth" value="true" type="bool"/>
<param name="objects_path" value="" type="str"/>
<param name="object_prefix" value="object" type="str"/>
<remap from="rgb/image_rect_color" to="/zed/rgb/image_rect_color"/>
<remap from="depth_registered/image_raw" to="/zed/depth/depth_registered"/>
<remap from="depth_registered/camera_info" to="/zed/rgb/camera_info"/>
</node>
</launch>

34
src/ros/CameraROS.cpp
View File

@ -137,13 +137,10 @@ void CameraROS::imgDepthReceivedCallback(
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
{
if(!(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)
{
ROS_ERROR("find_object_ros: Input type must be rgb=mono8,rgb8,bgr8 and depth=32FC1,16UC1");
ROS_ERROR("find_object_ros: Depth image type must be 32FC1 or 16UC1");
return;
}
@ -152,18 +149,27 @@ void CameraROS::imgDepthReceivedCallback(
cv_bridge::CvImageConstPtr ptr = cv_bridge::toCvShare(rgbMsg);
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
float depthConstant = 1.0f/cameraInfoMsg->K[4];
if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
cv::Mat image;
cv_bridge::CvImageConstPtr imgPtr = cv_bridge::toCvShare(rgbMsg);
try
{
cv::Mat cpy = ptr->image.clone();
Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
Q_EMIT imageReceived(cpy);
if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
rgbMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
image = cv_bridge::cvtColor(imgPtr, "mono8")->image;
}
else if(rgbMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0)
else
{
cv::Mat bgr;
cv::cvtColor(ptr->image, bgr, cv::COLOR_RGB2BGR);
image = cv_bridge::cvtColor(imgPtr, "bgr8")->image;
}
Q_EMIT rosDataReceived(rgbMsg->header.frame_id, rgbMsg->header.stamp, ptrDepth->image, depthConstant);
Q_EMIT imageReceived(bgr);
Q_EMIT imageReceived(image);
}
catch(const cv_bridge::Exception & e)
{
ROS_ERROR("find_object_ros: Could not convert input image to mono8 or bgr8 format, encoding detected is %s... cv_bridge exception: %s", rgbMsg->encoding.c_str(), e.what());
}
}