Convert Gazebo camera ImageStamped message to OpenCV Mat
Client code subscribe to gazebo image topic message type ImageStamped
and view the image as opencv mat
- Terminal 1 (run gazebo)
make posix_sitl_default gazebo_typhoon_h480
- Termianl 2 (view topics and topic message)
gz topic -l
#
/gazebo/default/typhoon_h480/camera/link/irlock
/gazebo/default/typhoon_h480/cgo3_camera_link/camera/cmd
/gazebo/default/typhoon_h480/cgo3_camera_link/camera/image
/gazebo/default/typhoon_h480/cgo3_camera_link/camera_imu/imu
/gazebo/default/typhoon_h480/cgo3_camera_link/wrench
/gazebo/default/typhoon_h480/cgo3_horizontal_arm_link/wr
#
gz topic -i /gazebo/default/typhoon_h480/cgo3_camera_link/camera/image
Type: gazebo.msgs.ImageStamped
- Terminal 2 (run the app)
cd build
cmake ..
make
../bin/viewer
- callback function
void cb(ConstImageStampedPtr &msg)
{
int width;
int height;
char *data;
width = (int) msg->image().width();
height = (int) msg->image().height();
//+1 for null terminate
data = new char[msg->image().data().length() + 1];
memcpy(data, msg->image().data().c_str(), msg->image().data().length());
//gazebo output rgb data
//PixelFormat.R8G8B8
cv::Mat image(height, width, CV_8UC3, data);
cv::imshow("camera", image);
cv::waitKey(1);
delete data; // DO NOT FORGET TO DELETE THIS,
// ELSE GAZEBO WILL TAKE ALL YOUR MEMORY
}
cmake_minimum_required(VERSION 2.8)
project(gz_viewer)
set(CMAKE_CXX_STANDARD 11)
set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
find_package( OpenCV REQUIRED )
add_executable(viewer viewer.cpp)
target_link_libraries(viewer ${OpenCV_LIBS} ${GAZEBO_LIBRARIES} pthread)
No comments:
Post a Comment