Skip to content

Commit

Permalink
Merge pull request IntelRealSense#8 from otcshare/RAR-179-dynamic-rec…
Browse files Browse the repository at this point in the history 
…onfigure

Rar 179 dynamic reconfigure
  • Loading branch information
@rjingar
rjingar committed Feb 25, 2016
2 parents cfb38ad + 77d7319 commit 46ac939
Show file tree
Hide file tree
Showing 6 changed files with 158 additions and 10 deletions.
10 changes: 9 additions & 1 deletion camera/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@ set(CMAKE_EXE_LINKER_FLAGS "-pie -z noexecstack -z relro -z now")
set(CMAKE_SHARED_LINKER_FLAGS "-z noexecstack -z relro -z now")

find_package(catkin REQUIRED COMPONENTS
dynamic_reconfigure
roscpp
nodelet
cv_bridge
Expand All @@ -37,6 +38,13 @@ generate_messages(
std_msgs
)

#add dynamic reconfigure api
generate_dynamic_reconfigure_options(
cfg/camera_params.cfg
)



include_directories(
${catkin_INCLUDE_DIRS}
)
Expand All @@ -46,7 +54,7 @@ target_link_libraries(realsense_camera_nodelet
${catkin_LIBRARIES}
/usr/local/lib/librealsense.so
)
add_dependencies(realsense_camera_nodelet realsense_camera_generate_messages_cpp)
add_dependencies(realsense_camera_nodelet realsense_camera_generate_messages_cpp ${PROJECT_NAME}_gencfg)

add_executable(realsense_camera_test test/realsense_camera_test_node.cpp)
target_link_libraries(realsense_camera_test
Expand Down
59 changes: 50 additions & 9 deletions camera/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,15 +28,6 @@ Sample launch files are available in camera/launch directory

<b>realsense_r200_rgbd.launch</b>

Note: The camera does not provide hardware based depth registration/projector data. Hence the launch file "realsense_r200_rgbd.launch" will not generate data for the following topics:
/camera/depth_registered/hw_registered/image_rect_raw
/camera/depth_registered/points
/camera/depth_registered/hw_registered/image_rect
/camera/depth_registered/image
/camera/depth/disparity
/camera/depth_registered/disparity


### Intel&reg; RealSense&trade; R200 Nodelet
Publishing stream data from the Intel® RealSense™ R200 (DS4) camera

Expand Down Expand Up @@ -135,6 +126,48 @@ Infrared2 camera
get_settings (camera/get_settings)
To get supported camera options with current value set. It returns string in options:value format where different options are seperated by semicolon.

####Dynamic Reconfiguration
List of dynamically configurable camera options:

COLOR_BACKLIGHT_COMPENSATION
COLOR_BRIGHTNESS
COLOR_CONTRAST
COLOR_GAIN
COLOR_GAMMA
COLOR_HUE
COLOR_SATURATION
COLOR_SHARPNESS
COLOR_WHITE_BALANCE
COLOR_ENABLE_AUTO_WHITE_BALANCE
R200_LR_AUTO_EXPOSURE_ENABLED
R200_LR_GAIN
R200_LR_EXPOSURE
R200_EMITTER_ENABLED
R200_DISPARITY_MULTIPLIER
R200_AUTO_EXPOSURE_TOP_EDGE
R200_AUTO_EXPOSURE_BOTTOM_EDGE
R200_AUTO_EXPOSURE_LEFT_EDGE
R200_AUTO_EXPOSURE_RIGHT_EDGE

Use rqt_reconfigure GUI to view and edit the parameters that are accessible via dynamic_reconfigure.

Command to launch GUI:

$ rosrun rqt_reconfigure rqt_reconfigure

Change options commandline using following command:

$ rosrun dynamic_reconfigure dynparam set /node parameter_name value
E.g. $ rosrun dynamic_reconfigure dynparam set /RealsenseNodelet COLOR_BACKLIGHT_COMPENSATION 2

Note: For Autoexposure EDGE parameters, max value will go only upto the bounds of the infrared image.
E.g. For 320x240 infrared image, valid values are within 0-319 and 0-239)

To change EDGE parameters, R200_LR_AUTO_EXPOSURE_ENABLED should be enabled.
To set R200_LR_EXPOSURE, R200_LR_AUTO_EXPOSURE_ENABLED should be disabled.
To set COLOR_WHITE_BALANCE, COLOR_ENABLE_AUTO_WHITE_BALANCE should be disabled.


###Running the R200 nodelet

Use the following command to launch the camera nodelet. You will notice the camera light up.
Expand Down Expand Up @@ -225,4 +258,12 @@ Currently, the ROS camera nodelet only supports the following formats:
* Depth stream: Y16
* Infrared stream: Y8

Note: The camera does not provide hardware based depth registration/projector data. Hence the launch file "realsense_r200_rgbd.launch" will not generate data for the following topics:
/camera/depth_registered/hw_registered/image_rect_raw
/camera/depth_registered/points
/camera/depth_registered/hw_registered/image_rect
/camera/depth_registered/image
/camera/depth/disparity
/camera/depth_registered/disparity


30 changes: 30 additions & 0 deletions camera/cfg/camera_params.cfg
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
#!/usr/bin/env python

PACKAGE="realsense_camera"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# Name Type Level Description Default Min Max
gen.add("COLOR_BACKLIGHT_COMPENSATION", int_t, 0, "Backlight Compensation", 1, 0, 4)
gen.add("COLOR_BRIGHTNESS", int_t, 0, "Brightness", 56, 0, 255)
gen.add("COLOR_CONTRAST", int_t, 0, "Contrast", 32, 16, 64)
gen.add("COLOR_GAIN", int_t, 0, "Gain", 32, 0, 256)
gen.add("COLOR_GAMMA", int_t, 0, "Gamma", 220, 100, 280)
gen.add("COLOR_HUE", int_t, 0, "Hue", 0, -2200, 2200)
gen.add("COLOR_SATURATION", int_t, 0, "Saturation", 128, 0, 255)
gen.add("COLOR_SHARPNESS", int_t, 0, "Sharpness", 0, 0, 7)
gen.add("COLOR_WHITE_BALANCE", int_t, 0, "White Balance", 6500, 2000, 8000)
gen.add("COLOR_ENABLE_AUTO_WHITE_BALANCE", int_t, 0, "Enable Auto White Balance", 1, 0, 1)
gen.add("R200_LR_AUTO_EXPOSURE_ENABLED", int_t, 0, "LR Auto Exposure Enabled", 0, 0, 1)
gen.add("R200_LR_GAIN", int_t, 0, "LR Gain", 400, 100, 6399)
gen.add("R200_LR_EXPOSURE", int_t, 0, "LR Exposure", 164, 1, 330)
gen.add("R200_EMITTER_ENABLED", int_t, 0, "Emitter Enabled", 1, 0, 1)
gen.add("R200_DISPARITY_MULTIPLIER", int_t, 0, "Disparity Multiplier", 32, 1, 1000)
gen.add("R200_AUTO_EXPOSURE_TOP_EDGE", int_t, 0, "Auto Exposure Top Edge", 0, 0, 479)
gen.add("R200_AUTO_EXPOSURE_BOTTOM_EDGE", int_t, 0, "Auto Exposure Bottom Edge", 479, 0, 479)
gen.add("R200_AUTO_EXPOSURE_LEFT_EDGE", int_t, 0, "Auto Exposure Left Edge", 0, 0, 639)
gen.add("R200_AUTO_EXPOSURE_RIGHT_EDGE", int_t, 0, "Auto Exposure Right Edge", 639, 0, 639)

exit(gen.generate(PACKAGE, "realsense_camera", "camera_params"))
2 changes: 2 additions & 0 deletions camera/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
<build_depend>sensor_msgs</build_depend>
<build_depend>rostest</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>dynamic_reconfigure</build_depend>

<run_depend>roscpp</run_depend>
<run_depend>nodelet</run_depend>
Expand All @@ -38,6 +39,7 @@
<run_depend>sensor_msgs</run_depend>
<run_depend>rostest</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>dynamic_reconfigure</run_depend>

<export>
<nodelet plugin="${prefix}/src/realsense_camera_nodelet_plugins.xml" />
Expand Down
54 changes: 54 additions & 0 deletions camera/src/realsense_camera_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,8 @@ namespace realsense_camera
}

get_options_service_ = nh.advertiseService (SETTINGS_SERVICE, &RealsenseNodelet::getCameraSettings, this);

dynamic_reconf_server_.reset(new dynamic_reconfigure::Server<realsense_camera::camera_paramsConfig>(getPrivateNodeHandle()));

bool connected = false;

Expand All @@ -137,11 +139,63 @@ namespace realsense_camera
{
ros::shutdown ();
}

dynamic_reconf_server_->setCallback(boost::bind(&RealsenseNodelet::configCallback, this, _1, _2));
}

/*
*Private Methods.
*/
void RealsenseNodelet::configCallback(realsense_camera::camera_paramsConfig &config, uint32_t level)
{
rs_set_device_option(rs_device_, RS_OPTION_COLOR_BACKLIGHT_COMPENSATION, config.COLOR_BACKLIGHT_COMPENSATION, 0);
rs_set_device_option(rs_device_, RS_OPTION_COLOR_BRIGHTNESS, config.COLOR_BRIGHTNESS, 0);
rs_set_device_option(rs_device_, RS_OPTION_COLOR_CONTRAST, config.COLOR_CONTRAST, 0);
rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAIN, config.COLOR_GAIN, 0);
rs_set_device_option(rs_device_, RS_OPTION_COLOR_GAMMA, config.COLOR_GAMMA, 0);
rs_set_device_option(rs_device_, RS_OPTION_COLOR_HUE, config.COLOR_HUE, 0);
rs_set_device_option(rs_device_, RS_OPTION_COLOR_SATURATION, config.COLOR_SATURATION, 0);
rs_set_device_option(rs_device_, RS_OPTION_COLOR_SHARPNESS, config.COLOR_SHARPNESS, 0);
rs_set_device_option(rs_device_, RS_OPTION_COLOR_ENABLE_AUTO_WHITE_BALANCE, config.COLOR_ENABLE_AUTO_WHITE_BALANCE, 0);

if(config.COLOR_ENABLE_AUTO_WHITE_BALANCE == 1) {
rs_set_device_option(rs_device_, RS_OPTION_COLOR_WHITE_BALANCE, config.COLOR_WHITE_BALANCE, 0);
}

//R200 camera specific options
rs_set_device_option(rs_device_, RS_OPTION_R200_LR_AUTO_EXPOSURE_ENABLED, config.R200_LR_AUTO_EXPOSURE_ENABLED, 0);

if(config.R200_LR_AUTO_EXPOSURE_ENABLED == 0) {
rs_set_device_option(rs_device_, RS_OPTION_R200_LR_EXPOSURE, config.R200_LR_EXPOSURE, 0);
}

rs_set_device_option(rs_device_, RS_OPTION_R200_LR_GAIN, config.R200_LR_GAIN, 0);
rs_set_device_option(rs_device_, RS_OPTION_R200_EMITTER_ENABLED, config.R200_EMITTER_ENABLED, 0);
rs_set_device_option(rs_device_, RS_OPTION_R200_DISPARITY_MULTIPLIER, config.R200_DISPARITY_MULTIPLIER, 0);

if(config.R200_LR_AUTO_EXPOSURE_ENABLED == 1)
{
if(config.R200_AUTO_EXPOSURE_TOP_EDGE >= depth_height_) {
config.R200_AUTO_EXPOSURE_TOP_EDGE = depth_height_ - 1;
}
if(config.R200_AUTO_EXPOSURE_BOTTOM_EDGE >= depth_height_) {
config.R200_AUTO_EXPOSURE_BOTTOM_EDGE = depth_height_ - 1;
}
if(config.R200_AUTO_EXPOSURE_LEFT_EDGE >= depth_width_) {
config.R200_AUTO_EXPOSURE_LEFT_EDGE = depth_width_ - 1;
}
if(config.R200_AUTO_EXPOSURE_RIGHT_EDGE >= depth_width_) {
config.R200_AUTO_EXPOSURE_RIGHT_EDGE = depth_width_ - 1;
}
edge_values_[0] = config.R200_AUTO_EXPOSURE_LEFT_EDGE;
edge_values_[1] = config.R200_AUTO_EXPOSURE_TOP_EDGE;
edge_values_[2] = config.R200_AUTO_EXPOSURE_RIGHT_EDGE;
edge_values_[3] = config.R200_AUTO_EXPOSURE_BOTTOM_EDGE;

rs_set_device_options(rs_device_, edge_options_, 4, edge_values_, 0);
}
}

void RealsenseNodelet::check_error ()
{
if (rs_error_)
Expand Down
13 changes: 13 additions & 0 deletions camera/src/realsense_camera_nodelet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,9 @@

#include <librealsense/rs.hpp>
#include <realsense_camera/cameraConfiguration.h>
#include <dynamic_reconfigure/server.h>
#include <realsense_camera/camera_paramsConfig.h>


namespace realsense_camera
{
Expand Down Expand Up @@ -129,6 +132,14 @@ class RealsenseNodelet: public nodelet::Nodelet

cv::Mat image_[STREAM_COUNT];

rs_option edge_options_[4] = {
RS_OPTION_R200_AUTO_EXPOSURE_LEFT_EDGE,
RS_OPTION_R200_AUTO_EXPOSURE_TOP_EDGE,
RS_OPTION_R200_AUTO_EXPOSURE_RIGHT_EDGE,
RS_OPTION_R200_AUTO_EXPOSURE_BOTTOM_EDGE
};
double edge_values_[4];

sensor_msgs::CameraInfoPtr camera_info_ptr_[STREAM_COUNT];
sensor_msgs::CameraInfo * camera_info_[STREAM_COUNT];
image_transport::CameraPublisher camera_publisher_[STREAM_COUNT];
Expand All @@ -149,6 +160,7 @@ class RealsenseNodelet: public nodelet::Nodelet
double min, max, step, value;
};
std::vector<option_str> options;
boost::shared_ptr<dynamic_reconfigure::Server<realsense_camera::camera_paramsConfig>> dynamic_reconf_server_;

// Member Functions.
void check_error();
Expand All @@ -165,6 +177,7 @@ class RealsenseNodelet: public nodelet::Nodelet
void getConfigValues(std::vector<std::string> args);
void setConfigValues(std::vector<std::string> args, std::vector<struct option_str> cam_options);
bool getCameraSettings(realsense_camera::cameraConfiguration::Request & req, realsense_camera::cameraConfiguration::Response & res);
void configCallback(realsense_camera::camera_paramsConfig &config, uint32_t level);

};
}
Expand Down

0 comments on commit 46ac939

Please sign in to comment.