From cbb2b5049e46b512e356161ba5d211569b18bb31 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Thu, 7 Jun 2018 12:04:29 -0500 Subject: [PATCH 1/4] Update topic names --- src/stereo_camera.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/stereo_camera.cpp b/src/stereo_camera.cpp index f4dfc008..09a32cac 100644 --- a/src/stereo_camera.cpp +++ b/src/stereo_camera.cpp @@ -35,7 +35,7 @@ namespace avt_vimba_camera { StereoCamera::StereoCamera(ros::NodeHandle nh, ros::NodeHandle nhp) -: nh_(nh), nhp_(nhp), it_(nh), left_cam_("left"), right_cam_("right") { +: nh_(nh), nhp_(nhp), it_(nhp), left_cam_("left"), right_cam_("right") { // Get the parameters nhp_.param("left_ip", left_ip_, std::string("")); @@ -63,8 +63,8 @@ void StereoCamera::run() { api_.start(); // Set the image publishers before the streaming - left_pub_ = it_.advertiseCamera("/stereo_down/left/image_raw", 1); - right_pub_ = it_.advertiseCamera("/stereo_down/right/image_raw", 1); + left_pub_ = it_.advertiseCamera("left/image_raw", 1); + right_pub_ = it_.advertiseCamera("right/image_raw", 1); // Set the frame callbacks left_cam_.setCallback(boost::bind(&avt_vimba_camera::StereoCamera::leftFrameCallback, this, _1)); @@ -86,8 +86,8 @@ void StereoCamera::run() { left_info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(ros::NodeHandle(nhp_, "left"),"left_optical",left_camera_info_url_)); right_info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(ros::NodeHandle(nhp_, "right"),"right_optical",right_camera_info_url_)); - pub_left_temp_ = nhp_.advertise("left_temp", 1, true); - pub_right_temp_ = nhp_.advertise("right_temp", 1, true); + pub_left_temp_ = nhp_.advertise("left/temp", 1, true); + pub_right_temp_ = nhp_.advertise("right/temp", 1, true); // Start dynamic_reconfigure & run configure() reconfigure_server_.setCallback(boost::bind(&StereoCamera::configure, this, _1, _2)); From ed560b3dc78d36cac425328ae627e8ba9f7c3515 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Thu, 7 Jun 2018 12:23:12 -0500 Subject: [PATCH 2/4] Fix duplicate launch install Update launch files for guids Add camera info url args Update launch and config --- CMakeLists.txt | 4 ---- cfg/AvtVimbaCameraStereo.cfg | 2 +- launch/mono_camera.launch | 3 ++- launch/stereo_camera_one_node.launch | 10 ++++++---- launch/stereo_camera_two_nodes.launch | 14 +++++++++----- 5 files changed, 18 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ffc947ba..8a143986 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -143,10 +143,6 @@ install(DIRECTORY include ## Mark other files for installation (e.g. launch and bag files, etc.) install(FILES plugins.xml - launch/mono_camera.launch - launch/mono_camera_nodelet.launch - launch/stereo_camera_one_node.launch - launch/stereo_camera_two_nodes.launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/cfg/AvtVimbaCameraStereo.cfg b/cfg/AvtVimbaCameraStereo.cfg index 3be23b47..bde84372 100755 --- a/cfg/AvtVimbaCameraStereo.cfg +++ b/cfg/AvtVimbaCameraStereo.cfg @@ -106,7 +106,7 @@ gen.add("right_frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING gen.add("right_trig_timestamp_topic", str_t, SensorLevels.RECONFIGURE_STOP, "Sets the topic from which an externally trigged camera receives its trigger timestamps.", "") # ACQUISITION gen.add("right_acquisition_mode", str_t, SensorLevels.RECONFIGURE_STOP, "Camera acquisition mode", "Continuous", edit_method = acquisition_mode_enum) -gen.add("right_acquisition_rate", double_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the expected triggering rate in externally triggered mode.", 2.0, 1.0, 30.0) +gen.add("right_acquisition_rate", double_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the expected triggering rate in externally triggered mode.", 10.0, 1.0, 30.0) # TRIGGER gen.add("right_trigger_source", str_t, SensorLevels.RECONFIGURE_STOP, "Camera trigger source", "FixedRate", edit_method = trigger_source_enum) gen.add("right_trigger_mode", str_t, SensorLevels.RECONFIGURE_STOP, "Camera trigger mode", "On", edit_method = trigger_mode_enum) diff --git a/launch/mono_camera.launch b/launch/mono_camera.launch index 529ab358..8d5cd41c 100644 --- a/launch/mono_camera.launch +++ b/launch/mono_camera.launch @@ -1,11 +1,12 @@ + - + diff --git a/launch/stereo_camera_one_node.launch b/launch/stereo_camera_one_node.launch index e3577d8a..16ce6d3b 100644 --- a/launch/stereo_camera_one_node.launch +++ b/launch/stereo_camera_one_node.launch @@ -2,7 +2,10 @@ - + + + + @@ -10,12 +13,11 @@ - - + + - diff --git a/launch/stereo_camera_two_nodes.launch b/launch/stereo_camera_two_nodes.launch index 0fdf7152..6264fdfa 100644 --- a/launch/stereo_camera_two_nodes.launch +++ b/launch/stereo_camera_two_nodes.launch @@ -1,5 +1,10 @@ + + + + + @@ -13,9 +18,9 @@ - + - + @@ -37,9 +42,9 @@ - + - + @@ -60,4 +65,3 @@ - From 461d527f59c7a64501edf50e69e8fa4b4e2225a3 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Fri, 8 Jun 2018 07:25:13 -0500 Subject: [PATCH 3/4] Fix camera info args --- launch/stereo_camera_one_node.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/stereo_camera_one_node.launch b/launch/stereo_camera_one_node.launch index 16ce6d3b..0d5cd577 100644 --- a/launch/stereo_camera_one_node.launch +++ b/launch/stereo_camera_one_node.launch @@ -2,11 +2,11 @@ - - + + - + From 2fc611c8484f0d024b753402450ed69da9828fd0 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Thu, 21 Jun 2018 12:55:39 -0500 Subject: [PATCH 4/4] Move dynamic reconfigure into private nh --- include/avt_vimba_camera/mono_camera.h | 2 +- include/avt_vimba_camera/stereo_camera.h | 2 +- src/mono_camera.cpp | 8 +++++--- src/stereo_camera.cpp | 16 +++++++++------- 4 files changed, 16 insertions(+), 12 deletions(-) diff --git a/include/avt_vimba_camera/mono_camera.h b/include/avt_vimba_camera/mono_camera.h index 97fb6109..a6c0f929 100644 --- a/include/avt_vimba_camera/mono_camera.h +++ b/include/avt_vimba_camera/mono_camera.h @@ -79,7 +79,7 @@ class MonoCamera { // Dynamic reconfigure typedef avt_vimba_camera::AvtVimbaCameraConfig Config; typedef dynamic_reconfigure::Server ReconfigureServer; - ReconfigureServer reconfigure_server_; + boost::shared_ptr reconfigure_server_; // Camera configuration Config camera_config_; diff --git a/include/avt_vimba_camera/stereo_camera.h b/include/avt_vimba_camera/stereo_camera.h index 8bd2cc1c..cd848033 100644 --- a/include/avt_vimba_camera/stereo_camera.h +++ b/include/avt_vimba_camera/stereo_camera.h @@ -108,7 +108,7 @@ class StereoCamera { typedef avt_vimba_camera::AvtVimbaCameraConfig Config; typedef avt_vimba_camera::AvtVimbaCameraStereoConfig StereoConfig; typedef dynamic_reconfigure::Server ReconfigureServer; - ReconfigureServer reconfigure_server_; + boost::shared_ptr reconfigure_server_; // Camera configuration StereoConfig camera_config_; diff --git a/src/mono_camera.cpp b/src/mono_camera.cpp index 73d1dc2f..b9bee0ba 100644 --- a/src/mono_camera.cpp +++ b/src/mono_camera.cpp @@ -36,7 +36,7 @@ namespace avt_vimba_camera { -MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp_(nhp), it_(nhp), cam_(ros::this_node::getName()) { +MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp_(nhp), it_(nh), cam_(ros::this_node::getName()) { // Prepare node handle for the camera // TODO use nodelets with getMTNodeHandle() @@ -60,8 +60,10 @@ MonoCamera::MonoCamera(ros::NodeHandle& nh, ros::NodeHandle& nhp) : nh_(nh), nhp // Set camera info manager info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(nhp_, frame_id, camera_info_url_)); - // Start dynamic_reconfigure & run configure() - reconfigure_server_.setCallback(boost::bind(&avt_vimba_camera::MonoCamera::configure, this, _1, _2)); + // Start up the dynamic_reconfigure service, note that this needs to stick around after this function ends + reconfigure_server_ = boost::make_shared (nhp_); + ReconfigureServer::CallbackType f = boost::bind(&avt_vimba_camera::MonoCamera::configure, this, _1, _2); + reconfigure_server_->setCallback(f); } MonoCamera::~MonoCamera(void) { diff --git a/src/stereo_camera.cpp b/src/stereo_camera.cpp index 09a32cac..8ae41edb 100644 --- a/src/stereo_camera.cpp +++ b/src/stereo_camera.cpp @@ -35,7 +35,7 @@ namespace avt_vimba_camera { StereoCamera::StereoCamera(ros::NodeHandle nh, ros::NodeHandle nhp) -: nh_(nh), nhp_(nhp), it_(nhp), left_cam_("left"), right_cam_("right") { +: nh_(nh), nhp_(nhp), it_(nh), left_cam_("left"), right_cam_("right") { // Get the parameters nhp_.param("left_ip", left_ip_, std::string("")); @@ -83,14 +83,16 @@ void StereoCamera::run() { updater_.update(); // Set camera info managers - left_info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(ros::NodeHandle(nhp_, "left"),"left_optical",left_camera_info_url_)); - right_info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(ros::NodeHandle(nhp_, "right"),"right_optical",right_camera_info_url_)); + left_info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(ros::NodeHandle(nh_, "left"),"left_optical",left_camera_info_url_)); + right_info_man_ = boost::shared_ptr(new camera_info_manager::CameraInfoManager(ros::NodeHandle(nh_, "right"),"right_optical",right_camera_info_url_)); - pub_left_temp_ = nhp_.advertise("left/temp", 1, true); - pub_right_temp_ = nhp_.advertise("right/temp", 1, true); + pub_left_temp_ = nh_.advertise("left/temp", 1, true); + pub_right_temp_ = nh_.advertise("right/temp", 1, true); - // Start dynamic_reconfigure & run configure() - reconfigure_server_.setCallback(boost::bind(&StereoCamera::configure, this, _1, _2)); + // Start up the dynamic_reconfigure service, note that this needs to stick around after this function ends + reconfigure_server_ = boost::make_shared (nhp_); + ReconfigureServer::CallbackType f = boost::bind(&StereoCamera::configure, this, _1, _2); + reconfigure_server_->setCallback(f); }