Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[22169] pkg/trunk/stacks
|
From: <pmi...@us...> - 2009-08-18 22:13:41
|
Revision: 22169
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=22169&view=rev
Author: pmihelich
Date: 2009-08-18 22:13:33 +0000 (Tue, 18 Aug 2009)
Log Message:
-----------
Updated forearm_cam and dcam to publish meaningful image encodings. stereo_image_proc now actually trusts the encoding from the camera driver.
Modified Paths:
--------------
pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp
pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp
pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h
pkg/trunk/stacks/stereo/stereo_image_proc/include/image.h
pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp
Modified: pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp 2009-08-18 22:01:13 UTC (rev 22168)
+++ pkg/trunk/stacks/camera_drivers/dcam/src/nodes/dcam.cpp 2009-08-18 22:13:33 UTC (rev 22169)
@@ -35,6 +35,7 @@
#include <cstdio>
#include "dcam/dcam.h"
+#include "cam_bridge.h"
#include "ros/node.h"
#include "sensor_msgs/Image.h"
@@ -199,70 +200,31 @@
publishImages("~", cam_->camIm);
}
- void publishImages(std::string base_name, cam::ImageData* img_data)
+ void publishImage(const std::string& topic, color_coding_t type,
+ int height, int width, void* data, size_t data_size)
{
- if (img_data->imRawType != COLOR_CODING_NONE)
- {
- fillImage(img_, sensor_msgs::image_encodings::TYPE_8UC1,
- img_data->imHeight, img_data->imWidth, img_data->imWidth,
- img_data->imRaw);
+ if (type == COLOR_CODING_NONE)
+ return;
+ std::string encoding = cam_bridge::ColorCodingToImageEncoding(type);
+ fillImage(img_, encoding, height, width, data_size / height, data);
+ img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
+ timestamp_diag_.tick(img_.header.stamp);
+ publish(topic, img_);
+ }
- img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
- timestamp_diag_.tick(img_.header.stamp);
- publish(base_name + std::string("image_raw"), img_);
- }
+ void publishImages(const std::string& base_name, cam::ImageData* img_data)
+ {
+ publishImage(base_name + "image_raw", img_data->imRawType, img_data->imHeight,
+ img_data->imWidth, img_data->imRaw, img_data->imRawSize);
+ publishImage(base_name + "image", img_data->imType, img_data->imHeight,
+ img_data->imWidth, img_data->im, img_data->imSize);
+ publishImage(base_name + "image_color", img_data->imColorType, img_data->imHeight,
+ img_data->imWidth, img_data->imColor, img_data->imColorSize);
+ publishImage(base_name + "image_rect", img_data->imRectType, img_data->imHeight,
+ img_data->imWidth, img_data->imRect, img_data->imRectSize);
+ publishImage(base_name + "image_rect_color", img_data->imRectColorType, img_data->imHeight,
+ img_data->imWidth, img_data->imRectColor, img_data->imRectColorSize);
- if (img_data->imType != COLOR_CODING_NONE)
- {
- fillImage(img_, sensor_msgs::image_encodings::TYPE_8UC1,
- img_data->imHeight, img_data->imWidth, img_data->imWidth,
- img_data->im);
- img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
- timestamp_diag_.tick(img_.header.stamp);
- publish(base_name + std::string("image"), img_);
- }
-
- if (img_data->imColorType != COLOR_CODING_NONE && img_data->imColorType == COLOR_CODING_RGB8)
- {
- fillImage(img_, sensor_msgs::image_encodings::TYPE_8UC3,
- img_data->imHeight, img_data->imWidth, 3 * img_data->imWidth,
- img_data->imColor );
-
- img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
- timestamp_diag_.tick(img_.header.stamp);
- publish(base_name + std::string("image_color"), img_);
- }
-
- if (img_data->imRectType != COLOR_CODING_NONE)
- {
- fillImage(img_, sensor_msgs::image_encodings::TYPE_8UC1,
- img_data->imHeight, img_data->imWidth, img_data->imWidth,
- img_data->imRect );
- img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
- timestamp_diag_.tick(img_.header.stamp);
- publish(base_name + std::string("image_rect"), img_);
- }
-
- if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGB8)
- {
- fillImage(img_, sensor_msgs::image_encodings::TYPE_8UC3,
- img_data->imHeight, img_data->imWidth, 3 * img_data->imWidth,
- img_data->imRectColor );
- img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
- timestamp_diag_.tick(img_.header.stamp);
- publish(base_name + std::string("image_rect_color"), img_);
- }
-
- if (img_data->imRectColorType != COLOR_CODING_NONE && img_data->imRectColorType == COLOR_CODING_RGBA8)
- {
- fillImage(img_,sensor_msgs::image_encodings::TYPE_8UC4,
- img_data->imHeight, img_data->imWidth, 4 * img_data->imWidth,
- img_data->imRectColor );
- img_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
- timestamp_diag_.tick(img_.header.stamp);
- publish(base_name + std::string("image_rect_color"), img_);
- }
-
cam_info_.header.stamp = ros::Time().fromNSec(cam_->camIm->im_time * 1000);
cam_info_.height = img_data->imHeight;
cam_info_.width = img_data->imWidth;
@@ -274,7 +236,6 @@
timestamp_diag_.tick(img_.header.stamp);
publish(base_name + std::string("cam_info"), cam_info_);
-
}
Modified: pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp
===================================================================
--- pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp 2009-08-18 22:01:13 UTC (rev 22168)
+++ pkg/trunk/stacks/camera_drivers/forearm_cam/src/nodes/forearm_node.cpp 2009-08-18 22:13:33 UTC (rev 22169)
@@ -957,7 +957,8 @@
int publishImage(size_t width, size_t height, uint8_t *frameData, ros::Time t)
{
- fillImage(image_, sensor_msgs::image_encodings::TYPE_8UC1, height, width, width, frameData);
+ // Raw data is bayer BGGR pattern
+ fillImage(image_, sensor_msgs::image_encodings::BAYER_BGGR8, height, width, width, frameData);
image_.header.stamp = t;
cam_pub_.publish(image_);
Modified: pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h
===================================================================
--- pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h 2009-08-18 22:01:13 UTC (rev 22168)
+++ pkg/trunk/stacks/stereo/stereo_image_proc/include/cam_bridge.h 2009-08-18 22:13:33 UTC (rev 22169)
@@ -37,45 +37,82 @@
#include "stereo_msgs/RawStereo.h"
#include "sensor_msgs/fill_image.h"
+#include "sensor_msgs/image_encodings.h"
#include "image.h"
namespace cam_bridge
{
+ color_coding_t GetColorCoding(const sensor_msgs::Image& msg)
+ {
+ using namespace sensor_msgs::image_encodings;
+
+ if (msg.encoding == MONO8) return COLOR_CODING_MONO8;
+ if (msg.encoding == MONO16) return COLOR_CODING_MONO16;
+ if (msg.encoding == BAYER_RGGB8) return COLOR_CODING_BAYER8_RGGB;
+ if (msg.encoding == BAYER_BGGR8) return COLOR_CODING_BAYER8_BGGR;
+ if (msg.encoding == BAYER_GBRG8) return COLOR_CODING_BAYER8_GBRG;
+ if (msg.encoding == BAYER_GRBG8) return COLOR_CODING_BAYER8_GRBG;
+ if (msg.encoding == RGB8) return COLOR_CODING_RGB8;
+ if (msg.encoding == RGBA8) return COLOR_CODING_RGBA8;
+
+ ROS_ERROR("cam_bridge: Encoding '%s' is not supported", msg.encoding.c_str());
+ return COLOR_CODING_NONE;
+ }
+
+ std::string ColorCodingToImageEncoding(color_coding_t coding)
+ {
+ using namespace sensor_msgs::image_encodings;
+
+ if (coding == COLOR_CODING_MONO8) return MONO8;
+ if (coding == COLOR_CODING_MONO16) return MONO16;
+ if (coding == COLOR_CODING_BAYER8_RGGB) return BAYER_RGGB8;
+ if (coding == COLOR_CODING_BAYER8_BGGR) return BAYER_BGGR8;
+ if (coding == COLOR_CODING_BAYER8_GBRG) return BAYER_GBRG8;
+ if (coding == COLOR_CODING_BAYER8_GRBG) return BAYER_GRBG8;
+ if (coding == COLOR_CODING_RGB8) return RGB8;
+ if (coding == COLOR_CODING_RGBA8) return RGBA8;
+
+ ROS_WARN("cam_bridge: Don't know image encoding string for color coding %i", coding);
+ return "";
+ }
+
void CamDataToRawStereo(cam::ImageData* im, sensor_msgs::Image& im_msg, sensor_msgs::CameraInfo& info_msg, uint8_t& type)
{
+ // @todo: this could all be less hard-coded
if (im->imRawType != COLOR_CODING_NONE)
{
- fillImage(im_msg, sensor_msgs::image_encodings::TYPE_8UC1, im->imHeight, im->imWidth, im->imWidth, im->imRaw);
+ std::string encoding = ColorCodingToImageEncoding(im->imRawType);
+ fillImage(im_msg, encoding, im->imHeight, im->imWidth, im->imWidth, im->imRaw);
type = stereo_msgs::RawStereo::IMAGE_RAW;
}
else if (im->imType != COLOR_CODING_NONE)
{
- fillImage(im_msg, sensor_msgs::image_encodings::TYPE_8UC1, im->imHeight, im->imWidth, im->imWidth, im->im);
+ fillImage(im_msg, sensor_msgs::image_encodings::MONO8, im->imHeight, im->imWidth, im->imWidth, im->im);
type = stereo_msgs::RawStereo::IMAGE;
}
else if (im->imColorType != COLOR_CODING_NONE && im->imColorType == COLOR_CODING_RGBA8)
{
- fillImage(im_msg, sensor_msgs::image_encodings::TYPE_8UC4, im->imHeight, im->imWidth, 4 * im->imWidth, im->imColor);
+ fillImage(im_msg, sensor_msgs::image_encodings::RGBA8, im->imHeight, im->imWidth, 4 * im->imWidth, im->imColor);
type = stereo_msgs::RawStereo::IMAGE_COLOR;
}
else if (im->imColorType != COLOR_CODING_NONE && im->imColorType == COLOR_CODING_RGB8)
{
- fillImage(im_msg, sensor_msgs::image_encodings::TYPE_8UC3, im->imHeight, im->imWidth, 3 * im->imWidth, im->imColor);
+ fillImage(im_msg, sensor_msgs::image_encodings::RGB8, im->imHeight, im->imWidth, 3 * im->imWidth, im->imColor);
type = stereo_msgs::RawStereo::IMAGE_COLOR;
}
else if (im->imRectType != COLOR_CODING_NONE)
{
- fillImage(im_msg, sensor_msgs::image_encodings::TYPE_8UC1, im->imHeight, im->imWidth, im->imWidth, im->imRect);
+ fillImage(im_msg, sensor_msgs::image_encodings::MONO8, im->imHeight, im->imWidth, im->imWidth, im->imRect);
type = stereo_msgs::RawStereo::IMAGE_RECT;
}
else if (im->imRectColorType != COLOR_CODING_NONE && im->imRectColorType == COLOR_CODING_RGBA8)
{
- fillImage(im_msg, sensor_msgs::image_encodings::TYPE_8UC4, im->imHeight, im->imWidth, 4 * im->imWidth, im->imRectColor);
+ fillImage(im_msg, sensor_msgs::image_encodings::RGBA8, im->imHeight, im->imWidth, 4 * im->imWidth, im->imRectColor);
type = stereo_msgs::RawStereo::IMAGE_RECT_COLOR;
}
else if (im->imRectColorType != COLOR_CODING_NONE && im->imRectColorType == COLOR_CODING_RGB8)
{
- fillImage(im_msg, sensor_msgs::image_encodings::TYPE_8UC3, im->imHeight, im->imWidth, 3 * im->imWidth, im->imRectColor);
+ fillImage(im_msg, sensor_msgs::image_encodings::RGB8, im->imHeight, im->imWidth, 3 * im->imWidth, im->imRectColor);
type = stereo_msgs::RawStereo::IMAGE_RECT_COLOR;
}
@@ -175,49 +212,34 @@
if (type == stereo_msgs::RawStereo::IMAGE_RAW)
{
extractImage(im_msg.data, &im->imRawSize, &im->imRaw);
- im->imRawType = COLOR_CODING_BAYER8_GRBG;
+ im->imRawType = GetColorCoding(im_msg);
}
else if (type == stereo_msgs::RawStereo::IMAGE)
{
extractImage(im_msg.data, &im->imSize, &im->im);
im->imType = COLOR_CODING_MONO8;
}
- else if (type == stereo_msgs::RawStereo::IMAGE_COLOR && im_msg.encoding == sensor_msgs::image_encodings::TYPE_8UC4)
+ else if (type == stereo_msgs::RawStereo::IMAGE_COLOR)
{
extractImage(im_msg.data, &im->imColorSize, &im->imColor);
- im->imColorType = COLOR_CODING_RGBA8;
+ im->imColorType = GetColorCoding(im_msg);
}
- else if (type == stereo_msgs::RawStereo::IMAGE_COLOR && im_msg.encoding == sensor_msgs::image_encodings::TYPE_8UC3)
- {
- extractImage(im_msg.data, &im->imColorSize, &im->imColor);
- im->imColorType = COLOR_CODING_RGB8;
- }
else if (type == stereo_msgs::RawStereo::IMAGE_RECT)
{
extractImage(im_msg.data, &im->imRectSize, &im->imRect);
- im->imRectType = COLOR_CODING_MONO8;
+ im->imRectType = GetColorCoding(im_msg);
}
- else if (type == stereo_msgs::RawStereo::IMAGE_RECT_COLOR && im_msg.encoding == sensor_msgs::image_encodings::TYPE_8UC4)
+ else if (type == stereo_msgs::RawStereo::IMAGE_RECT_COLOR)
{
extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
- im->imRectColorType = COLOR_CODING_RGBA8;
+ im->imRectColorType = GetColorCoding(im_msg);
}
- else if (type == stereo_msgs::RawStereo::IMAGE_RECT_COLOR && im_msg.encoding == sensor_msgs::image_encodings::TYPE_8UC3)
- {
- extractImage(im_msg.data, &im->imRectColorSize, &im->imRectColor);
- im->imRectColorType = COLOR_CODING_RGB8;
- }
- // FIXME: this to avoid segfault when right image empty (disparity image requested instead).
- // this all seems kind of hacky
- if (im_msg.encoding == sensor_msgs::image_encodings::TYPE_8UC1) {
- im->imHeight = im_msg.height;
- im->imWidth = im_msg.width;
- } else {
- im->imHeight = info_msg.height;
- im->imWidth = info_msg.width;
- }
+ // @todo: this OK when right image empty (disparity image requested instead)?
+ im->imHeight = im_msg.height;
+ im->imWidth = im_msg.width;
+ // @todo: possible to NOT have rectification?
memcpy((char*)(im->D), (char*)(&info_msg.D[0]), 5*sizeof(double));
memcpy((char*)(im->K), (char*)(&info_msg.K[0]), 9*sizeof(double));
memcpy((char*)(im->R), (char*)(&info_msg.R[0]), 9*sizeof(double));
@@ -232,7 +254,8 @@
stIm->setSize(raw_stereo.stereo_info.width, raw_stereo.stereo_info.height);
stIm->hasDisparity = false;
-
+
+ // @todo: if not, don't try to extract right image?
if (raw_stereo.has_disparity)
{
extractImage(raw_stereo.disparity_image.data, &stIm->imDispSize, &stIm->imDisp);
Modified: pkg/trunk/stacks/stereo/stereo_image_proc/include/image.h
===================================================================
--- pkg/trunk/stacks/stereo/stereo_image_proc/include/image.h 2009-08-18 22:01:13 UTC (rev 22168)
+++ pkg/trunk/stacks/stereo/stereo_image_proc/include/image.h 2009-08-18 22:13:33 UTC (rev 22169)
@@ -176,6 +176,7 @@
// the Type info is COLOR_CODING_NONE if the data is not current
// the Size info gives the buffer size, for allocation logic
// NOTE: all data buffers should be 16-byte aligned
+ // @todo: can we just use IplImages for these...
uint8_t *imRaw; // raw image
color_coding_t imRawType; // type of raw data
size_t imRawSize;
Modified: pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp
===================================================================
--- pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp 2009-08-18 22:01:13 UTC (rev 22168)
+++ pkg/trunk/stacks/stereo/stereo_image_proc/src/imageproc.cpp 2009-08-18 22:13:33 UTC (rev 22169)
@@ -76,7 +76,7 @@
node_.subscribe(cam_name_ + "image_raw", raw_img_, &ImageProc::rawCb, this, 1);
}
- // TODO: should the callbacks actually be synchronized? is there a use case?
+ // @todo: synchronize callbacks
void camInfoCb()
{
boost::lock_guard<boost::mutex> guard(cam_info_mutex_);
@@ -85,11 +85,10 @@
void rawCb()
{
+ // @todo: move publishing into here, only do processing if topics have subscribers
boost::lock_guard<boost::mutex> guard(cam_info_mutex_);
cam_bridge::RawStereoToCamData(raw_img_, cam_info_, stereo_msgs::RawStereo::IMAGE_RAW, &img_data_);
- // @todo: stop hardcoding this to what forearm cam happens to use
- img_data_.imRawType = COLOR_CODING_BAYER8_BGGR;
if (do_colorize_) {
//img_data_.colorConvertType = COLOR_CONVERSION_EDGE;
@@ -103,50 +102,27 @@
publishImages();
}
+ void publishImage(color_coding_t coding, void* data, size_t dataSize, const std::string& topic)
+ {
+ if (coding == COLOR_CODING_NONE)
+ return;
+
+ // @todo: step calculation is a little hacky
+ fillImage(img_, cam_bridge::ColorCodingToImageEncoding(coding),
+ img_data_.imHeight, img_data_.imWidth,
+ dataSize / img_data_.imHeight /*step*/, data);
+ node_.publish(cam_name_ + topic, img_);
+ }
+
void publishImages()
{
img_.header.stamp = raw_img_.header.stamp;
img_.header.frame_id = raw_img_.header.frame_id;
-
- if (img_data_.imType != COLOR_CODING_NONE)
- {
- fillImage(img_, sensor_msgs::image_encodings::MONO8,
- img_data_.imHeight, img_data_.imWidth, img_data_.imWidth,
- img_data_.im );
- node_.publish(cam_name_ + "image", img_);
- }
- if (img_data_.imColorType == COLOR_CODING_RGB8)
- {
- fillImage(img_,sensor_msgs::image_encodings::RGB8,
- img_data_.imHeight, img_data_.imWidth, 3 * img_data_.imWidth,
- img_data_.imColor );
- node_.publish(cam_name_ + "image_color", img_);
- }
-
- if (img_data_.imRectType != COLOR_CODING_NONE)
- {
- fillImage(img_, sensor_msgs::image_encodings::MONO8,
- img_data_.imHeight, img_data_.imWidth, img_data_.imWidth,
- img_data_.imRect );
- node_.publish(cam_name_ + "image_rect", img_);
- }
-
- if (img_data_.imRectColorType == COLOR_CODING_RGB8)
- {
- fillImage(img_, sensor_msgs::image_encodings::RGB8,
- img_data_.imHeight, img_data_.imWidth, 3 * img_data_.imWidth,
- img_data_.imRectColor );
- node_.publish(cam_name_ + "image_rect_color", img_);
- }
-
- if (img_data_.imRectColorType == COLOR_CODING_RGBA8)
- {
- fillImage(img_, sensor_msgs::image_encodings::RGBA8,
- img_data_.imHeight, img_data_.imWidth, 4 * img_data_.imWidth,
- img_data_.imRectColor);
- node_.publish(cam_name_ + "image_rect_color", img_);
- }
+ publishImage(img_data_.imType, img_data_.im, img_data_.imSize, "image");
+ publishImage(img_data_.imColorType, img_data_.imColor, img_data_.imColorSize, "image_color");
+ publishImage(img_data_.imRectType, img_data_.imRect, img_data_.imRectSize, "image_rect");
+ publishImage(img_data_.imRectColorType, img_data_.imRectColor, img_data_.imRectColorSize, "image_rect_color");
}
void advertiseImages()
@@ -161,7 +137,7 @@
int main(int argc, char **argv)
{
ros::init(argc, argv);
- ros::Node n("stereoproc");
+ ros::Node n("imageproc");
ImageProc ip(n);
n.spin();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
