diff --git a/.gitignore b/.gitignore index 3f94b114..ec03a124 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ ## folders ## ############# build +devel cmake-* ############# diff --git a/LUCIDCamerasWithROS_v1.1.pdf b/LUCIDCamerasWithROS_v1.1.pdf deleted file mode 100644 index 07ef72ef..00000000 Binary files a/LUCIDCamerasWithROS_v1.1.pdf and /dev/null differ diff --git a/README.md b/README.md index 0afacc4f..58d22583 100644 --- a/README.md +++ b/README.md @@ -1,32 +1,8 @@ -Arena Camera deriver for ROS +## Arena Camera Driver for ROS1 +# Getting Started +setup and usage https://support.thinklucid.com/using-ros-for-linux/ -## Getting Started -### Prerequisites -* Ubuntu-16.04 xenial -* ROS kinetic -* ArenaSDK binraies - -### Basic Setup -#### Environment for ArenaSDK - -```shell -echo "export ARENA_ROOT=">> ~/.bashrc -echo "export ARENA_ROOT=">> ~/.zshrc -source ~/.bashrc -source ~/.zshrc -``` - -#### Workspace -Copy the included image_encoding.h to your ROS include folder after -backing up the old one (if existed). The custom image_encoding.h is -included to enable streaming support for LUCID’s Helios camera. -```shell -sudo cp -f \ - /opt/ros/kinetic/include/sensor_msgs/image_encodings.h \ - /opt/ros/kinetic/include/sensor_msgs/image_encodings.h.bak -sudo cp -f \ - /catkin_ws/inc/image_encodings.h \ - /opt/ros/kinetic/include/sensor_msgs/image_encodings.h -``` +for ROS2 driver contact LUCID support for more information +https://support.thinklucid.com/contact-support/ diff --git a/catkin_ws/.catkin_workspace b/catkin_ws/.catkin_workspace new file mode 100644 index 00000000..52fd97e7 --- /dev/null +++ b/catkin_ws/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/catkin_ws/src/arena_camera/.vscode/c_cpp_properties.json b/catkin_ws/src/arena_camera/.vscode/c_cpp_properties.json new file mode 100644 index 00000000..06d08770 --- /dev/null +++ b/catkin_ws/src/arena_camera/.vscode/c_cpp_properties.json @@ -0,0 +1,15 @@ +{ + "configurations": [ + { + "name": "ROS", + "includePath": [], // reads from the seetingsin workspace file + "defines": [], // reads from the seetingsin workspace file + "compilerPath": "/usr/bin/gcc", + "cStandard": "c11", + "cppStandard": "c++11", + //"configurationProvider": "b2.catkin_tools", + "intelliSenseMode": "gcc-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/catkin_ws/src/arena_camera/CMakeLists.txt b/catkin_ws/src/arena_camera/CMakeLists.txt index 8d6a28f3..e3621710 100644 --- a/catkin_ws/src/arena_camera/CMakeLists.txt +++ b/catkin_ws/src/arena_camera/CMakeLists.txt @@ -1,26 +1,51 @@ -cmake_minimum_required(VERSION 2.8.3) +# http://wiki.ros.org/catkin/CMakeLists.txt +# using catkin must follow the structure and order ectribed in the file above +# ----------------------------------------------------------------------------- # -# DEV NOTE -# - relative path are relative to this file -# +# 1 +# +# ----------------------------------------------------------------------------- -project(arena_camera) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +cmake_minimum_required(VERSION 2.8.3) -# flags for all C++ targets -add_definitions("-std=gnu++11") +# ----------------------------------------------------------------------------- +# +# 2 +# +# ----------------------------------------------------------------------------- + +project(arena_camera) # ----------------------------------------------------------------------------- # -# CATKIN +# 3 # # ----------------------------------------------------------------------------- + +# +# ARENA SDK +# + +# ADDS : +# - ${_arena_sdk_ROOT} +# - ${arena_sdk_INCLUDE_DIRS} and ${arena_sdk_INCLUDES} +# - ${arena_sdk_LIBRARIES_DIRS} and ${arena_sdk_LIBS} +# - ${arena_sdk_FOUND} + +find_package(arena_sdk QUIET) +if (NOT ${arena_sdk_FOUND}) + include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/Findarena_sdk.cmake") +endif() + +# +# CATKIN # + set(CATKIN_COMPONENTS actionlib - camera_control_msgs + camera_control_msgs # arena_camera depends on this upper custom msg package camera_info_manager cv_bridge diagnostic_updater @@ -36,212 +61,224 @@ set(CATKIN_COMPONENTS ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package( - catkin REQUIRED + catkin + REQUIRED COMPONENTS ${CATKIN_COMPONENTS} ) -# http://wiki.ros.org/catkin/CMakeLists.txt#catkin_package.28.29 -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS ${CATKIN_COMPONENTS} -) # ----------------------------------------------------------------------------- # -# ROS +# 4 # # ----------------------------------------------------------------------------- -# -set(ROSLINT_CPP_OPTS - "--extensions=cpp,h,hpp" "--filter=-runtime/references,-readability/todo,-build/include_what_you_use" -) -# check c/c++ static checking. More http://wiki.ros.org/roslint -roslint_cpp() # all .h .cpp files +# This macro ensures modules and global scripts declared therein get installed +# - Enable Python module support +# - See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() -# closest doc (http://wiki.ros.org/rosbuild/CMakeLists#rosbuild_add_roslaunch_check) -# more https://answers.ros.org/question/200359/purpose-of-roslaunch_add_file_check/ -roslaunch_add_file_check(launch) # ----------------------------------------------------------------------------- # -# ARENA -# TODO -# Make the dirs read from Arena_SDK.conf +# 5 # # ----------------------------------------------------------------------------- -# -# ADDS : -# - ${_ARENA_ROOT} -# - ${Arena_INCLUDE_DIRS} -# - ${Arena_LIBRARIES} -# - ${Arena_LIBRARIES_DIRS} -set(_ARENA_ROOT "$ENV{ARENA_ROOT}") -set(Arena_INCLUDE_DIRS - "${_ARENA_ROOT}/GenICam/library/CPP/include" -) -set(Arena_LIBRARIES - "arena" - "save" - "gentl" - "GenApi_gcc421_v3_0" - "GCBase_gcc421_v3_0" -) -set(Arena_LIBRARIES_DIRS -"${_ARENA_ROOT}/lib64/" -"${_ARENA_ROOT}/GenICam/library/lib/Linux64_x64/" -) + +# N/A + # ----------------------------------------------------------------------------- # -# GENERAL +# 6 # # ----------------------------------------------------------------------------- -# -# headers for compiler -include_directories( - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${catkin_INCLUDE_DIRS} - ${Arena_INCLUDE_DIRS} -) - -link_directories( - ${Arena_LIBRARIES_DIRS} -) +# N/A # ----------------------------------------------------------------------------- # -# ARENA_CAMERA +# 7 # # ----------------------------------------------------------------------------- -# -# create a virtual tree for this project -# generates *.o -# https://cmake.org/cmake/help/latest/command/add_library.html -add_library( - ${PROJECT_NAME} - src/${PROJECT_NAME}_node.cpp - src/${PROJECT_NAME}_parameter.cpp - src/${PROJECT_NAME}.cpp - src/encoding_conversions.cpp +# http://wiki.ros.org/catkin/CMakeLists.txt#catkin_package.28.29 +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_COMPONENTS} # package dep for build/run + # DEPENDS # sys dep for build/run + # <> + # <> ) -# catkin first then arena_camera -# the first argument must be created before using add_library -# https://cmake.org/cmake/help/latest/command/add_dependencies.html -add_dependencies( - ${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} +#7 +# ROS CHECKS +# + +set(ROSLINT_CPP_OPTS + "--extensions=cpp,h,hpp" "--filter=-runtime/references,-readability/todo,-build/include_what_you_use" ) -target_link_libraries( - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Arena_LIBRARIES} +# check c/c++ static checking. More http://wiki.ros.org/roslint +# no files mean all files +roslint_cpp( # all .h .cpp files ) +# closest doc (http://wiki.ros.org/rosbuild/CMakeLists#rosbuild_add_roslaunch_check) +# more https://answers.ros.org/question/200359/purpose-of-roslaunch_add_file_check/ +roslaunch_add_file_check(launch) + + # ----------------------------------------------------------------------------- # -# ARENA_CAMERA_NODE +# 8 # # ----------------------------------------------------------------------------- + + +# flags for all C++ targets +add_definitions("-std=gnu++11") +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + + # +# ARENA_CAMERA_NODE +# + +set(arena_camera_node_name arena_camera_node) -# compile the cpp and create an executable +# - declare an executable +# - needed ".cpp"s add_executable( - ${PROJECT_NAME}_node # runs by rosrun - src/main.cpp + ${arena_camera_node_name} # runs by rosrun + src/arena_camera_node.cpp + src/arena_camera_parameter.cpp + src/arena_camera.cpp + src/encoding_conversions.cpp + src/main.cpp ) +# needed ".h"s +target_include_directories( + ${arena_camera_node_name} + PRIVATE ${catkin_INCLUDE_DIRS} + PRIVATE ${arena_sdk_INCLUDE_DIRS} + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +# needed ".so"s target_link_libraries( - ${PROJECT_NAME}_node - ${catkin_LIBRARIES} - ${Arena_LIBRARIES} - ${PROJECT_NAME} + ${arena_camera_node_name} + ${catkin_LIBRARIES} + ${arena_sdk_LIBRARIES} ) -# ----------------------------------------------------------------------------- +add_dependencies( + ${arena_camera_node_name} + # because this uses one of the catkin componenets which is camera_control_msg + ${catkin_EXPORTED_TARGETS} +) + +# target_link_directories NOT NEEDED as arena_sdk_LIBRARIES is +# define with absolute paths + + # # WRITE_DEVICE_USER_ID_TO_CAMERA # -# ----------------------------------------------------------------------------- -# -# compile the cpp and create an executable +set(write_device_user_id_to_camera write_device_user_id_to_camera) + +# - declare an executable +# - needed ".cpp"s add_executable( - write_device_user_id_to_camera - src/write_device_user_id_to_camera.cpp + ${write_device_user_id_to_camera} # run by rosrun + src/write_device_user_id_to_camera.cpp +) + +# needed ".h"s +target_include_directories( + ${write_device_user_id_to_camera} + PRIVATE ${arena_sdk_INCLUDE_DIRS} + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include ) +# needed ".so"s target_link_libraries( - write_device_user_id_to_camera - ${catkin_LIBRARIES} - ${Arena_LIBRARIES} - ${PROJECT_NAME} + ${write_device_user_id_to_camera} + ${arena_sdk_LIBRARIES} ) -# This macro ensures modules and global scripts declared therein get installed -# See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() + +# target_link_directories NOT NEEDED as arena_sdk_LIBRARIES is +# define with absolute paths # ----------------------------------------------------------------------------- # -# INSTALL +# 9 # # ----------------------------------------------------------------------------- + +# N/A + +# ----------------------------------------------------------------------------- +# +# 10 # +# ----------------------------------------------------------------------------- install( DIRECTORY - launch/ + launch/ DESTINATION - ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch FILES_MATCHING PATTERN "*.launch" ) install( DIRECTORY - config/ + config/ DESTINATION - ${CATKIN_PACKAGE_SHARE_DESTINATION}/config + ${CATKIN_PACKAGE_SHARE_DESTINATION}/config FILES_MATCHING PATTERN "*.yaml" ) install( PROGRAMS - scripts/file_sequencer.py - scripts/grab_and_save_image_action_server.py - scripts/result_bag_to_action.py - scripts/sequence_to_file.py - scripts/toggle_camera + scripts/file_sequencer.py + scripts/grab_and_save_image_action_server.py + scripts/result_bag_to_action.py + scripts/sequence_to_file.py + scripts/toggle_camera + scripts/triggered_image_topic.py DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) install( TARGETS - ${PROJECT_NAME} - ${PROJECT_NAME}_node - write_device_user_id_to_camera + ${arena_camera_node_name} + ${write_device_user_id_to_camera} LIBRARY DESTINATION - ${CATKIN_PACKAGE_LIB_DESTINATION} + ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION} + ${CATKIN_PACKAGE_BIN_DESTINATION} ) install( DIRECTORY - include/${PROJECT_NAME}/ + include/${PROJECT_NAME}/ DESTINATION - ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN - "*.h" + "*.h" PATTERN "internal" EXCLUDE ) diff --git a/catkin_ws/src/arena_camera/cmake/Findarena_sdk.cmake b/catkin_ws/src/arena_camera/cmake/Findarena_sdk.cmake new file mode 100755 index 00000000..5f76e3db --- /dev/null +++ b/catkin_ws/src/arena_camera/cmake/Findarena_sdk.cmake @@ -0,0 +1,101 @@ + +set(_LOG_LVL DEBUG) # does not filter as it should +set(_LOG_LVL_FRMT "-- [ ${_LOG_LVL} ] ") + +# the installation script place +set(_arena_sdk_conf "/etc/ld.so.conf.d/Arena_SDK.conf") + +if(EXISTS ${_arena_sdk_conf}) + + ###### -------------------------------------------------------------------- + # ROOT + ###### + + # get first line in Arena_SDK.conf which is the lib64 path. then get the + # parent direcotry of the first path which suppose to be the location of + # the installed ArenaSDK + execute_process( + COMMAND bash -c "dirname $(head -n 1 \"/etc/ld.so.conf.d/Arena_SDK.conf\")" + OUTPUT_VARIABLE arena_sdk_installation_root + #ENCODING UTF8 + ) + string(STRIP ${arena_sdk_installation_root} arena_sdk_installation_root) + + #message(${_LOG_LVL_FRMT} "arena_sdk_installation_root = ${arena_sdk_installation_root}") + + ######### ----------------------------------------------------------------- + # INCLUDE + ######### + + set(arena_sdk_INCLUDE_DIRS + ${arena_sdk_installation_root}/GenICam/library/CPP/include + ${arena_sdk_installation_root}/include/Arena) + set (arena_sdk_INCLUDES ${arena_sdk_INCLUDE_DIRS}) + + #message(${_LOG_LVL_FRMT} "arena_sdk_INCLUDE_DIRS = ${arena_sdk_INCLUDE_DIRS}") + + ###### -------------------------------------------------------------------- + # LIBS + ###### + + set(arena_sdk_LIBS + + ## ArenaSDK + #${arena_sdk_installation_root}/lib64/libsave.so + #${arena_sdk_installation_root}/lib64/libsaved.so + + ${arena_sdk_installation_root}/lib64/libarena.so + #${arena_sdk_installation_root}/lib64/libarenad.so + + ${arena_sdk_installation_root}/lib64/libgentl.so + #${arena_sdk_installation_root}/lib64/libgentld.so + + #${arena_sdk_installation_root}/lib64/liblucidlog.so + #${arena_sdk_installation_root}/lib64/liblucidlogd.so + + ## GenICam + ${arena_sdk_installation_root}/GenICam/library/lib/Linux64_x64/libGCBase_gcc421_v3_0.so + ${arena_sdk_installation_root}/GenICam/library/lib/Linux64_x64/libGenApi_gcc421_v3_0.so + #${arena_sdk_installation_root}/GenICam/library/lib/Linux64_x64/liblog4cpp_gcc421_v3_0.so + #${arena_sdk_installation_root}/GenICam/library/lib/Linux64_x64/libLog_gcc421_v3_0.so + #${arena_sdk_installation_root}/GenICam/library/lib/Linux64_x64/libMathParser_gcc421_v3_0.so + #${arena_sdk_installation_root}/GenICam/library/lib/Linux64_x64/libNodeMapData_gcc421_v3_0.so + #${arena_sdk_installation_root}/GenICam/library/lib/Linux64_x64/libXmlParser_gcc421_v3_0.so + ## fmpeg + #${arena_sdk_installation_root}/ffmpeg/libavcodec.so + #${arena_sdk_installation_root}/ffmpeg/libavformat.so + #${arena_sdk_installation_root}/ffmpeg/libavutil.so + #${arena_sdk_installation_root}/ffmpeg/libswresample.so + ) + + set(arena_sdk_LIBRARIES ${arena_sdk_LIBS}) + #set(arena_sdk_DEFINITIONS GENICAM_USER_ACCEPTS_ANY_COMPILER) + + #message(${_LOG_LVL_FRMT} "arena_sdk_LIBRARIES = ${arena_sdk_LIBRARIES}") + + ###### -------------------------------------------------------------------- + # LIBS DIRS + ###### + # NOT NEEDED AS _LIBS GIVES THE ABSOLUTE PATH TO THE SOs + #set(arena_sdk_LIBRARIES_DIRS + # # ArenaSDK + # ${arena_sdk_installation_root}/lib64 + # ${arena_sdk_installation_root}/GenICam/library/lib/Linux64_x64/ + # ${arena_sdk_installation_root}/ffmpeg/ + #) + + #message(${_LOG_LVL_FRMT} "arena_sdk_LIBRARIES_DIRS = ${arena_sdk_LIBRARIES_DIRS}") + + ####### ------------------------------------------------------------------- + # FOUND + ####### + + set(arena_sdk_FOUND true) + + #message(${_LOG_LVL_FRMT} "arena_sdk_FOUND = ${arena_sdk_FOUND}") + +else() + message( FATAL_ERROR "ArenaSDK is not installed. Please isntall ArenaSDK " + "using the script provided by LUCID support " + "team (support@thinklucid.com). ") +endif() \ No newline at end of file diff --git a/catkin_ws/src/arena_camera/include/arena_camera/arena_camera_node.h b/catkin_ws/src/arena_camera/include/arena_camera/arena_camera_node.h index 02693f86..ea0db0ed 100644 --- a/catkin_ws/src/arena_camera/include/arena_camera/arena_camera_node.h +++ b/catkin_ws/src/arena_camera/include/arena_camera/arena_camera_node.h @@ -29,34 +29,41 @@ #ifndef ARENA_CAMERA_ARENA_CAMERA_NODE_H #define ARENA_CAMERA_ARENA_CAMERA_NODE_H -#include -#include -#include -#include -#include -#include + +// STD +#include + +// ROS sys dep +#include + +// ROS #include #include #include -#include -#include -#include -#include +#include +#include -#include #include +#include #include #include #include #include #include #include - #include + +#include +#include +#include +#include +#include -#include -#include +// Arena +#include +#include +#include namespace arena_camera { diff --git a/catkin_ws/src/arena_camera/package.xml b/catkin_ws/src/arena_camera/package.xml index 0804237b..659a87d9 100644 --- a/catkin_ws/src/arena_camera/package.xml +++ b/catkin_ws/src/arena_camera/package.xml @@ -62,7 +62,7 @@ diagnostic_updater image_geometry image_transport - arena + arena_sdk roscpp sensor_msgs roslaunch @@ -75,7 +75,7 @@ cv_bridge image_geometry image_transport - arena + arena_sdk roscpp roslaunch sensor_msgs diff --git a/catkin_ws/src/arena_camera/src/arena_camera_node.cpp b/catkin_ws/src/arena_camera/src/arena_camera_node.cpp index 9a39701e..90d23683 100644 --- a/catkin_ws/src/arena_camera/src/arena_camera_node.cpp +++ b/catkin_ws/src/arena_camera/src/arena_camera_node.cpp @@ -55,6 +55,7 @@ using sensor_msgs::CameraInfoPtr; ArenaCameraNode::ArenaCameraNode() : nh_("~") , arena_camera_parameter_set_() + // srv init , set_binning_srv_(nh_.advertiseService("set_binning", &ArenaCameraNode::setBinningCallback, this)) , set_roi_srv_(nh_.advertiseService("set_roi", &ArenaCameraNode::setROICallback, this)) , set_exposure_srv_(nh_.advertiseService("set_exposure", &ArenaCameraNode::setExposureCallback, this)) @@ -63,7 +64,9 @@ ArenaCameraNode::ArenaCameraNode() , set_brightness_srv_(nh_.advertiseService("set_brightness", &ArenaCameraNode::setBrightnessCallback, this)) , set_sleeping_srv_(nh_.advertiseService("set_sleeping", &ArenaCameraNode::setSleepingCallback, this)) , set_user_output_srvs_() + // Arena , arena_camera_(nullptr) + // others , it_(new image_transport::ImageTransport(nh_)) , img_raw_pub_(it_->advertiseCamera("image_raw", 1)) , img_rect_pub_(nullptr) @@ -72,7 +75,7 @@ ArenaCameraNode::ArenaCameraNode() , grab_imgs_rect_as_(nullptr) , pinhole_model_(nullptr) , cv_bridge_img_rect_(nullptr) - , camera_info_manager_(new camera_info_manager::CameraInfoManager(nh_)) + , camera_info_manager_(new camera_info_manager::CameraInfoManager(nh_)) // should this be freed in ~() ? , sampling_indices_() , brightness_exp_lut_() , is_sleeping_(false) @@ -346,9 +349,12 @@ bool ArenaCameraNode::setImageEncoding(const std::string& ros_encoding) } else { + std::string fallbackPixelFormat = Arena::GetNodeValue(pDevice_->GetNodeMap(), "PixelFormat").c_str(); ROS_ERROR_STREAM("Can't convert ROS encoding '" << ros_encoding << "' to a corresponding GenAPI encoding! Will use current " - << "pixel format as fallback!"); + << "pixel format ( " + << fallbackPixelFormat + << " ) as fallback!"); return false; } } @@ -373,19 +379,143 @@ bool ArenaCameraNode::setImageEncoding(const std::string& ros_encoding) bool ArenaCameraNode::startGrabbing() { + auto pNodeMap = pDevice_->GetNodeMap(); + try { + // + // Arena device prior streaming settings + // + + // + // PIXELFORMAT + // setImageEncoding(arena_camera_parameter_set_.imageEncoding()); - GenApi::CStringPtr pTriggerMode = pDevice_->GetNodeMap()->GetNode("TriggerMode"); + // + // TRIGGER MODE + // + GenApi::CStringPtr pTriggerMode = pNodeMap->GetNode("TriggerMode"); if (GenApi::IsWritable(pTriggerMode)) { - Arena::SetNodeValue(pDevice_->GetNodeMap(), "TriggerMode", "On"); - Arena::SetNodeValue(pDevice_->GetNodeMap(), "TriggerSource", "Software"); + Arena::SetNodeValue(pNodeMap, "TriggerMode", "On"); + Arena::SetNodeValue(pNodeMap, "TriggerSource", "Software"); + } + + // + // FRAMERATE + // + auto cmdlnParamFrameRate = arena_camera_parameter_set_.frameRate(); + auto currentFrameRate = Arena::GetNodeValue(pNodeMap , "AcquisitionFrameRate"); + auto maximumFrameRate = GenApi::CFloatPtr(pNodeMap->GetNode("AcquisitionFrameRate"))->GetMax(); + + // requested framerate larger than device max so we trancate it + if (cmdlnParamFrameRate >= maximumFrameRate) + { + arena_camera_parameter_set_.setFrameRate(nh_, maximumFrameRate); + + ROS_WARN("Desired framerate %.2f Hz (rounded) is higher than max possible. Will limit " + "framerate device max : %.2f Hz (rounded)", cmdlnParamFrameRate, maximumFrameRate); + } + // special case: + // dues to inacurate float comparision we skip. If we set it it might + // throw becase it could be a lil larger than the max avoid the exception (double accuracy issue when setting the node) + // request frame rate very close to device max + else if (cmdlnParamFrameRate == maximumFrameRate){ + ROS_INFO("Framerate is %.2f Hz", cmdlnParamFrameRate); + } + // requested max frame rate + else if (cmdlnParamFrameRate == -1) // speacial for max frame rate available + { + arena_camera_parameter_set_.setFrameRate(nh_, maximumFrameRate); + + ROS_WARN("Framerate is set to device max : %.2f Hz", maximumFrameRate); + } + // requested framerate is valid so we set it to the device + else{ + Arena::SetNodeValue(pNodeMap, "AcquisitionFrameRateEnable", true); + Arena::SetNodeValue(pNodeMap, "AcquisitionFrameRate" , + cmdlnParamFrameRate); + ROS_INFO("Framerate is set to: %.2f Hz", cmdlnParamFrameRate); + } + + // + // EXPOSURE AUTO & EXPOSURE + // + + // exposure_auto_ will be already set to false if exposure_given_ is true + // read params () solved the priority between them + if (arena_camera_parameter_set_.exposure_auto_) + { + Arena::SetNodeValue(pNodeMap, "ExposureAuto", "Continuous"); + // todo update parameter on the server + ROS_INFO_STREAM("Settings Exposure to auto/Continuous"); + } + else + { + Arena::SetNodeValue(pNodeMap, "ExposureAuto", "Off"); + // todo update parameter on the server + ROS_INFO_STREAM("Settings Exposure to off/false"); } + if (arena_camera_parameter_set_.exposure_given_) + { + float reached_exposure; + if (setExposure(arena_camera_parameter_set_.exposure_, reached_exposure)) + { + // Note: ont update the ros param because it might keep + // decreasing or incresing overtime when rerun + ROS_INFO_STREAM("Setting exposure to " << arena_camera_parameter_set_.exposure_ + << ", reached: " << reached_exposure); + } + } + + // + // GAIN + // + + // gain_auto_ will be already set to false if gain_given_ is true + // read params () solved the priority between them + if (arena_camera_parameter_set_.gain_auto_) + { + Arena::SetNodeValue(pNodeMap, "GainAuto", "Continuous"); + // todo update parameter on the server + ROS_INFO_STREAM("Settings Gain to auto/Continuous"); + } + else + { + Arena::SetNodeValue(pNodeMap, "GainAuto", "Off"); + // todo update parameter on the server + ROS_INFO_STREAM("Settings Gain to off/false"); + } + + if (arena_camera_parameter_set_.gain_given_) + { + float reached_gain; + if (setGain(arena_camera_parameter_set_.gain_, reached_gain)) + { + // Note: ont update the ros param because it might keep + // decreasing or incresing overtime when rerun + ROS_INFO_STREAM("Setting gain to: " << arena_camera_parameter_set_.gain_ << ", reached: " << reached_gain); + } + } + + // + // GAMMA // - // // Initial setting of the CameraInfo-msg, assuming no calibration given + if (arena_camera_parameter_set_.gamma_given_) + { + float reached_gamma; + if (setGamma(arena_camera_parameter_set_.gamma_, reached_gamma)) + { + ROS_INFO_STREAM("Setting gamma to " << arena_camera_parameter_set_.gamma_ << ", reached: " << reached_gamma); + } + } + + // ------------------------------------------------------------------------ + + // + // Initial setting of the CameraInfo-msg, assuming no calibration given CameraInfo initial_cam_info; setupInitialCameraInfo(initial_cam_info); camera_info_manager_->setCameraInfo(initial_cam_info); @@ -399,7 +529,7 @@ bool ArenaCameraNode::startGrabbing() ROS_DEBUG_STREAM("CameraInfoURL should have following style: " << "'file:///full/path/to/local/file.yaml' or " << "'file://${ROS_HOME}/camera_info/${NAME}.yaml'"); - ROS_WARN("Will only provide distorted /image_raw images!"); + ROS_WARN_STREAM("Will only provide distorted /image_raw images!"); } else { @@ -414,7 +544,7 @@ bool ArenaCameraNode::startGrabbing() } else { - ROS_WARN("Will only provide distorted /image_raw images!"); + ROS_WARN_STREAM("Will only provide distorted /image_raw images!"); } } @@ -440,21 +570,6 @@ bool ArenaCameraNode::startGrabbing() } } - if (arena_camera_parameter_set_.exposure_given_) - { - float reached_exposure; - if (setExposure(arena_camera_parameter_set_.exposure_, reached_exposure)) - { - ROS_INFO_STREAM("Setting exposure to " << arena_camera_parameter_set_.exposure_ - << ", reached: " << reached_exposure); - } - } - else if (arena_camera_parameter_set_.brightness_given_ && arena_camera_parameter_set_.exposure_auto_) - { - Arena::SetNodeValue(pDevice_->GetNodeMap(), "ExposureAuto", "Continuous"); - ROS_INFO_STREAM("Settings Exposure to auto"); - } - // if (arena_camera_parameter_set_.image_encoding_given_) // { // float reached_image_encoding; @@ -465,26 +580,12 @@ bool ArenaCameraNode::startGrabbing() // } // } - if (arena_camera_parameter_set_.gain_given_) - { - float reached_gain; - if (setGain(arena_camera_parameter_set_.gain_, reached_gain)) - { - ROS_INFO_STREAM("Setting gain to: " << arena_camera_parameter_set_.gain_ << ", reached: " << reached_gain); - } - } - - if (arena_camera_parameter_set_.gamma_given_) - { - float reached_gamma; - if (setGamma(arena_camera_parameter_set_.gamma_, reached_gamma)) - { - ROS_INFO_STREAM("Setting gamma to " << arena_camera_parameter_set_.gamma_ << ", reached: " << reached_gamma); - } - } - Arena::SetNodeValue(pDevice_->GetTLStreamNodeMap(), "StreamBufferHandlingMode", "NewestOnly"); + // + // Trigger Image + // + pDevice_->StartStream(); bool isTriggerArmed = false; @@ -492,9 +593,9 @@ bool ArenaCameraNode::startGrabbing() { do { - isTriggerArmed = Arena::GetNodeValue(pDevice_->GetNodeMap(), "TriggerArmed"); + isTriggerArmed = Arena::GetNodeValue(pNodeMap, "TriggerArmed"); } while (isTriggerArmed == false); - Arena::ExecuteNode(pDevice_->GetNodeMap(), "TriggerSoftware"); + Arena::ExecuteNode(pNodeMap, "TriggerSoftware"); } pImage_ = pDevice_->GetImage(5000); @@ -509,6 +610,8 @@ bool ArenaCameraNode::startGrabbing() return false; } + // -------------------------------------------------------------------------- + img_raw_msg_.header.frame_id = arena_camera_parameter_set_.cameraFrame(); // Encoding of pixels -- channel meaning, ordering, size // taken from the list of strings in include/sensor_msgs/image_encodings.h @@ -520,11 +623,11 @@ bool ArenaCameraNode::startGrabbing() img_raw_msg_.step = img_raw_msg_.width * (pImage_->GetBitsPerPixel() / 8); if (!camera_info_manager_->setCameraName( - std::string(Arena::GetNodeValue(pDevice_->GetNodeMap(), "DeviceUserID").c_str()))) + std::string(Arena::GetNodeValue(pNodeMap, "DeviceUserID").c_str()))) { // valid name contains only alphanumeric signs and '_' ROS_WARN_STREAM( - "[" << std::string(Arena::GetNodeValue(pDevice_->GetNodeMap(), "DeviceUserID").c_str()) + "[" << std::string(Arena::GetNodeValue(pNodeMap, "DeviceUserID").c_str()) << "] name not valid for camera_info_manager"); } @@ -538,23 +641,6 @@ bool ArenaCameraNode::startGrabbing() << "gamma = " << currentGamma() << ", " << "shutter mode = " << arena_camera_parameter_set_.shutterModeString()); - // Framerate Settings - pNodeMap_ = pDevice_->GetNodeMap(); - auto maximumFrameRate = GenApi::CFloatPtr(pNodeMap_->GetNode("AcquisitionFrameRate"))->GetMax(); - - if (maximumFrameRate < arena_camera_parameter_set_.frameRate()) - { - ROS_INFO("Desired framerate %.2f is higher than max possible. Will limit " - "framerate to: %.2f Hz", - arena_camera_parameter_set_.frameRate(), maximumFrameRate); - arena_camera_parameter_set_.setFrameRate(nh_, maximumFrameRate); - } - else if (arena_camera_parameter_set_.frameRate() == -1) - { - arena_camera_parameter_set_.setFrameRate(nh_, maximumFrameRate); - ROS_INFO("Max possible framerate is %.2f Hz", maximumFrameRate); - } - pDevice_->RequeueBuffer(pImage_); return true; } @@ -1072,6 +1158,8 @@ void ArenaCameraNode::setupInitialCameraInfo(sensor_msgs::CameraInfo& cam_info_m // the same window of pixels on the camera sensor, regardless of binning // settings. The default setting of roi (all values 0) is considered the same // as full resolution (roi.width = width, roi.height = height). + + // todo? do these has ti be set via Arena::GetNodeValue(pDevice_->GetNodeMap(), "OffsetX"); or so ? cam_info_msg.roi.x_offset = cam_info_msg.roi.y_offset = 0; cam_info_msg.roi.height = cam_info_msg.roi.width = 0; } diff --git a/catkin_ws/src/arena_camera/src/arena_camera_parameter.cpp b/catkin_ws/src/arena_camera/src/arena_camera_parameter.cpp index 058ba080..bfaf3e9c 100644 --- a/catkin_ws/src/arena_camera/src/arena_camera_parameter.cpp +++ b/catkin_ws/src/arena_camera/src/arena_camera_parameter.cpp @@ -27,9 +27,15 @@ * POSSIBILITY OF SUCH DAMAGE. *****************************************************************************/ -#include +// STD +#include // std::boolalpha + +// ROS #include +// Arena ROS +#include + namespace arena_camera { ArenaCameraParameter::ArenaCameraParameter() @@ -83,6 +89,7 @@ void ArenaCameraParameter::readFromRosParameterServer(const ros::NodeHandle& nh) if (nh.hasParam("frame_rate")) { nh.getParam("frame_rate", frame_rate_); + ROS_DEBUG_STREAM("frame_rate is given and has value " << frame_rate_); } nh.param("camera_info_url", camera_info_url_, ""); @@ -96,7 +103,7 @@ void ArenaCameraParameter::readFromRosParameterServer(const ros::NodeHandle& nh) { int binning_x; nh.getParam("binning_x", binning_x); - std::cout << "binning x is given and has value " << binning_x << std::endl; + ROS_DEBUG_STREAM( "binning x is given and has value " << binning_x); if (binning_x > 32 || binning_x < 0) { ROS_WARN_STREAM("Desired horizontal binning_x factor not in valid " @@ -114,7 +121,7 @@ void ArenaCameraParameter::readFromRosParameterServer(const ros::NodeHandle& nh) { int binning_y; nh.getParam("binning_y", binning_y); - std::cout << "binning y is given and has value " << binning_y << std::endl; + ROS_DEBUG_STREAM("binning y is given and has value " << binning_y); if (binning_y > 32 || binning_y < 0) { ROS_WARN_STREAM("Desired vertical binning_y factor not in valid " @@ -152,61 +159,180 @@ void ArenaCameraParameter::readFromRosParameterServer(const ros::NodeHandle& nh) // image intensity settings // ########################## + gamma_given_ = nh.hasParam("gamma"); + if (gamma_given_) + { + nh.getParam("gamma", gamma_); + ROS_DEBUG_STREAM("gamma is given and has value " << gamma_); + } + // > 0: Exposure time in microseconds exposure_given_ = nh.hasParam("exposure"); + brightness_given_ = nh.hasParam("brightness"); + gain_given_ = nh.hasParam("gain"); if (exposure_given_) { nh.getParam("exposure", exposure_); - std::cout << "exposure is given and has value " << exposure_ << std::endl; + ROS_DEBUG_STREAM("exposure is given and has value " << exposure_); } - - gain_given_ = nh.hasParam("gain"); if (gain_given_) { nh.getParam("gain", gain_); - std::cout << "gain is given and has value " << gain_ << std::endl; + ROS_DEBUG_STREAM("gain is given and has value " << gain_ ); + } + if (brightness_given_) + { + nh.getParam("brightness", brightness_); + ROS_DEBUG_STREAM("brightness is given and has value " << brightness_); } - gamma_given_ = nh.hasParam("gamma"); - if (gamma_given_) + // ignore brightness? + auto ignoreBrightness = brightness_given_ && gain_given_ && exposure_given_; + if (ignoreBrightness) { - nh.getParam("gamma", gamma_); - std::cout << "gamma is given and has value " << gamma_ << std::endl; + ROS_WARN_STREAM("Gain ('gain') and Exposure Time ('exposure') " + << "are given as startup ros-parameter and hence assumed to be " + << "fix! The desired brightness (" << brightness_ << ") can't " + << "be reached! Will ignore the brightness by only " + << "setting gain and exposure . . ."); + brightness_given_ = false; + } + else if (nh.hasParam("brightness_continuous")) + { + nh.getParam("brightness_continuous", brightness_continuous_); + ROS_DEBUG_STREAM("brightness is continuous"); } + + // ignore exposure_auto ? + /* + exposure_given | exposure_auto_given_ | exposure_auto_ | action | + | | received val | | + ---------------|----------------------|----------------|------------------| + 1 F F F | default value issue | + + 2 F F T | default case notting to do | + + 3 F T F | shoe msg ; and set exposure_auto true in nodemap | + + 4 F T T | print param msg + + 5 T F F | default value issue | + + 6 T F T | set default exposure_auto to false silently | + + 7 T T F | show param msg + + 8 T T T | show ignore msg; show param msg ;set to false | + */ + auto exposure_auto_given = nh.hasParam("exposure_auto"); + nh.getParam("exposure_auto", exposure_auto_); + + //1 FFF (exposure_auto_ 's default value is not set to true) + + // 2 FFT + if (!exposure_given_ && !exposure_auto_given && exposure_auto_) + { + // default case nothing to show/do + } + // 3 FTF + else if(!exposure_given_ && exposure_auto_given && !exposure_auto_){ + // it is ok to pass exposure_auto explicitly to false + // with no exposure time. Exposure time will taken from device nodemap + ROS_DEBUG_STREAM("exposure_auto is given and has value Off/false" ); - brightness_given_ = nh.hasParam("brightness"); - if (brightness_given_) + // TODO SET ON THE NODE MAP + } + // 4 FTT + else if(!exposure_given_ && exposure_auto_given && exposure_auto_){ + ROS_DEBUG_STREAM("exposure_auto is given and has value Continuous/true"); + } + + // 5 TFF (exposure_auto_ 's default value is not set true) + + // 6 TFT + else if(exposure_given_ && !exposure_auto_given && exposure_auto_) { - nh.getParam("brightness", brightness_); - std::cout << "brightness is given and has value " << brightness_ << std::endl; - if (gain_given_ && exposure_given_) - { - ROS_WARN_STREAM("Gain ('gain') and Exposure Time ('exposure') " - << "are given as startup ros-parameter and hence assumed to be " - << "fix! The desired brightness (" << brightness_ << ") can't " - << "be reached! Will ignore the brightness by only " - << "setting gain and exposure . . ."); - brightness_given_ = false; - } - else - { - if (nh.hasParam("brightness_continuous")) - { - nh.getParam("brightness_continuous", brightness_continuous_); - std::cout << "brightness is continuous" << std::endl; - } - if (nh.hasParam("exposure_auto")) - { - nh.getParam("exposure_auto", exposure_auto_); - std::cout << "exposure is set to auto" << std::endl; - } - if (nh.hasParam("gain_auto")) - { - nh.getParam("gain_auto", gain_auto_); - std::cout << "gain is set to auto" << std::endl; - } - } + exposure_auto_ = false; // change because it defaults to true; + //no msg it is not take from the param server + } + // 7 TTF + else if(exposure_given_ && exposure_auto_given && !exposure_auto_){ + ROS_DEBUG_STREAM("exposure_auto is given and has value Off/false" ); } + // 8 TTT + else if(exposure_given_ && exposure_auto_given && exposure_auto_) // ignore auto + { + ROS_DEBUG_STREAM("exposure_auto is given and has value Continuous/true"); + exposure_auto_ = false; + ROS_WARN_STREAM("exposure_auto is ignored because exposure is given."); + } + + // ignore gain_auto + /* + gain_given | gain_auto_given_ | gain_auto_ | action | + | | received val | | + ---------------|----------------------|----------------|------------------| + 1 F F F | default value issue | + + 2 F F T | default case notting to do | + + 3 F T F | shoe msg ; and set gain_auto true in nodemap | + + 4 F T T | print param msg + + 5 T F F | default value issue | + + 6 T F T | set default gain_auto to false silently | + + 7 T T F | show param msg + + 8 T T T | show ignore msg; show param msg ;set to false | + */ + + auto gain_auto_given = nh.hasParam("gain_auto"); + nh.getParam("gain_auto", gain_auto_); + + //1 FFF (gain_auto_ 's default value is not set to true) + + // 2 FFT + if (!gain_given_ && !gain_auto_given && gain_auto_) + { + // default case nothing to show/do + } + // 3 FTF + else if(!gain_given_ && gain_auto_given && !gain_auto_){ + // it is ok to pass gain_auto explicitly to false + // with no gain value. Gain value will taken from device nodemap + ROS_DEBUG_STREAM("gain_auto is given and has value Off/false" ); + + // TODO SET ON THE NODE MAP + } + // 4 FTT + else if(!gain_given_ && gain_auto_given && gain_auto_){ + ROS_DEBUG_STREAM("gain_auto is given and has value Continuous/true"); + } + + // 5 TFF (gain_auto_ 's default value is not set true) + + // 6 TFT + else if(gain_given_ && !gain_auto_given && gain_auto_) + { + gain_auto_ = false; // change because it defaults to true; + //no msg it is not take from the param server + } + // 7 TTF + else if(gain_given_ && gain_auto_given && !gain_auto_){ + ROS_DEBUG_STREAM("gain_auto is given and has value Off/false" ); + } + // 8 TTT + else if (gain_given_ && gain_auto_given && gain_auto_) // ignore auto + { + ROS_DEBUG_STREAM("gain_auto is given and has value Continuous/true"); + gain_auto_ = false; + ROS_WARN_STREAM("gain_auto is ignored because gain is given."); + } + + // ########################## nh.param("exposure_search_timeout", exposure_search_timeout_, 5.); diff --git a/ros_and_workspace_setup.sh b/ros_and_workspace_setup.sh index e0dab214..e4ad900b 100644 --- a/ros_and_workspace_setup.sh +++ b/ros_and_workspace_setup.sh @@ -3,9 +3,9 @@ # # MAIN VARS TO CHANGE # -ARENA_INSTALLATION_ROOT="$HOME/ArenaSDK_Linux_x64" +ARENA_INSTALLATION_ROOT="$HOME/ArenaSDK_Linux_x64/" ARENA_ROS_WORDSPACE_TO_SETUP="$HOME/arena_camera_ros/catkin_ws" #change to workspace location -INSTALL_ROS=0 +INSTALL_ROS=1 @@ -16,6 +16,7 @@ INSTALL_ROS=0 # ############################################################ + set -x #echo on # only run with sudo @@ -30,7 +31,7 @@ CURR_OS="$(lsb_release -sc)" if [ $CURR_OS = "xenial" ]; then ROS_DIS="kinetic" else - echo "$CURR_OS is not saupported yet" + echo "$CURR_OS is might not be supported yet! check https://support.thinklucid.com/using-ros-for-linux/" exit(-1) fi @@ -43,6 +44,7 @@ if [ $INSTALL_ROS -eq 1 ]; then # Set up your system to acquire software from packages.ros.org sudo echo "deb http://packages.ros.org/ros/ubuntu $CURR_OS main" > /etc/apt/sources.list.d/ros-latest.list # Use apt-key to install the Open Robotics key to your list of trusted keys. + # to remove the key at uninstaltion time run `sudo apt-key del F42ED6FBAB17C654` sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 # Install ROS Desktop. sudo apt-get update @@ -53,14 +55,14 @@ if [ $INSTALL_ROS -eq 1 ]; then rosdep update # Setups ROS environment variables - echo "# load ROS env vars" >> ~/.bashrc - echo "source /opt/ros/$ROS_DIS/setup.bash" >> ~/.bashrc + #echo "# load ROS env vars" >> ~/.bashrc + #echo "source /opt/ros/$ROS_DIS/setup.bash" >> ~/.bashrc - echo "# load ROS env vars" >> ~/.zshrc - echo "source /opt/ros/$ROS_DIS/setup.zsh" >> ~/.zshrc + #echo "# load ROS env vars" >> ~/.zshrc + #echo "source /opt/ros/$ROS_DIS/setup.zsh" >> ~/.zshrc # would not have an effect if script is not run in intractive mode - source ~/.bashrc + #source ~/.bashrc # # Install ROS package workspace dependencies. This will allow @@ -84,10 +86,10 @@ fi # Set up your ARENA_ROOT environment variable. This # environment variable should be the path where you have # installed Arena SDK. -echo "export ARENA_ROOT=$ARENA_INSTALLATION_ROOT">> ~/.bashrc -echo "export ARENA_ROOT=$ARENA_INSTALLATION_ROOT">> ~/.zshrc +#echo "export ARENA_ROOT=$ARENA_INSTALLATION_ROOT">> ~/.bashrc +#echo "export ARENA_ROOT=$ARENA_INSTALLATION_ROOT">> ~/.zshrc # would not have an effect if script is not run in intractive mode -source ~/.bashrc +#source ~/.bashrc ############################################################ # Workspace section @@ -102,4 +104,4 @@ sudo cp -f \ /opt/ros/$ROS_DIS/include/sensor_msgs/image_encodings.h.bak sudo cp -f \ $ARENA_ROS_WORDSPACE_TO_SETUP/inc/image_encodings.h \ - /opt/ros/$ROS_DIS/include/sensor_msgs/image_encodings.h \ No newline at end of file + /opt/ros/$ROS_DIS/include/sensor_msgs/image_encodings.h diff --git a/ros_arenasd.code-workspace b/ros_arenasd.code-workspace new file mode 100644 index 00000000..12bfef6d --- /dev/null +++ b/ros_arenasd.code-workspace @@ -0,0 +1,195 @@ +{ + "folders": [ + { + "name": "ARENA_CAMERA_PKG", + "path": "./catkin_ws/src/arena_camera" + }, + { + "name": "WS_ROOT", + "path": "catkin_ws" + }, + { + "name": "REPO_ROOT", + "path": "." + }, + { + "name": "SFW_INSTALLED", + "path": "../software_release_chckout" + } + ], + "extensions": { + "recommendations": [ + "ms-iot.vscode-ros", + "ms-vscode.cpptools", + "xaver.clang-format", + "streetsidesoftware.code-spell-checker", + "twxs.cmake", + "ms-vscode.cmake-tools", + "cheshirekow.cmake-format", + "ms-python.vscode-pylance" + ] + }, + "launch": { + "configurations": [ + //--------------------------------------------------------------------- + // + // My Args + // + //--------------------------------------------------------------------- + { + "name": "arena_camera_node with args", + "type": "cppdbg", + "MIMode": "gdb", + "setupCommands": [ + { + "text": "-enable-pretty-printing" // for std::string to show value on houver while steppingg + } + ], + "request": "launch", + "program": "${workspaceFolder:WS_ROOT}/devel/lib/arena_camera/arena_camera_node", + "args": [], + "cwd": "${workspaceFolder:WS_ROOT}", + //"stopAtEntry": true, + "stopAtEntry": false, + "preLaunchTask": "catkin_build_debug", + //"preLaunchTask": "catkin_clean_build_debug", + "sourceFileMap": { + //"/home/ubuntu/Jenkins/workspace/ArenaSDK_Master_Lin/": "${workspaceFolder:SFW_INSTALLED}/" + } + }, + ], + "compounds": [] + }, + "tasks": { + "version": "2.0.0", + "tasks": [ + /*{ + "type": "catkin_make", + "problemMatcher": [ + "$catkin-gcc" + ], + "group": { + "kind": "build", + "isDefault": false + }, + "label": "catkin_make: build" + },*/ + { + "label": "catkin_clean_build_debug", + "type": "shell", + "problemMatcher": [ + "$catkin-gcc" + ], + "dependsOn": [ + "set_params" + ], + "command": [ + "cd ${workspaceFolder:REPO_ROOT}/catkin_ws/", + ";", + "rm", + "-rf", + "build", + "devel", + "install", + "/src/arena_camera_node/build", + "/src/build", + ";", + "catkin_make -DCMAKE_BUILD_TYPE=Debug" + ], + "group": { + "kind": "build", + "isDefault": true + }, + }, + { + "label": "catkin_build_debug", + "type": "shell", + "problemMatcher": [ + "$catkin-gcc" + ], + "dependsOn": [ + "set_params" + ], + "command": [ + "cd ${workspaceFolder:REPO_ROOT}/catkin_ws/", + ";", + "catkin_make -DCMAKE_BUILD_TYPE=Debug" + ] + }, + { + "label": "set_params", + "type": "shell", + "command": [ + ";rosparam delete /arena_camera_node/", + ";rosparam set /arena_camera_node/frame_rate", + "10.9", + ";rosparam set /arena_camera_node/image_encoding", + "mono8", + //";rosparam set /arena_camera_node/device_user_id","", + //";rosparam set /arena_camera_node/binning_x","", + //";rosparam set /arena_camera_node/binning_y","", + //";rosparam set /arena_camera_node/gamma","", + ";rosparam set /arena_camera_node/exposure", + "100", + ";rosparam set /arena_camera_node/exposure_auto", + "true", + //";rosparam set /arena_camera_node/gain", + //"4", + //";rosparam set /arena_camera_node/gain_auto","" + ] + }, + ] + }, + "settings": { + // terminal + // + "terminal.integrated.cwd": "${workspaceFolder:REPO_ROOT}/catkin_ws/", + // + // file + // + "files.eol": "\n", + // + // C/CPP + // + "[cpp]": { + "editor.defaultFormatter": "xaver.clang-format", + }, + "C_Cpp.clang_format_fallbackStyle": "none", + "C_Cpp.clang_format_sortIncludes": true, + "C_Cpp.workspaceSymbols": "All", + "C_Cpp.loggingLevel": "Information", + "C_Cpp.intelliSenseCacheSize": 2000, // Megabytes + "C_Cpp.intelliSenseEngine": "Default", + "C_Cpp.intelliSenseEngineFallback": "Disabled", + "C_Cpp.default.includePath": [ + "/opt/ros/kinetic/include/", // ros installation dir + "${env:ARENA_ROOT}/**" // ArenaSDK installation dir. you can replace the env variable with a path + ], + "C_Cpp.default.browse.path": [], + "C_Cpp.default.enableConfigurationSquiggles": true, + "C_Cpp.dimInactiveRegions": true, + "C_Cpp.autocomplete": "Default", + "C_Cpp.default.configurationProvider": "${default}", + // + // Cmake + // + "cmake.parallelJobs": 88, + "cmake.saveBeforeBuild": true, + "cmake.parseBuildDiagnostics": true, + // ${workspaceFolder} interpreted PKG_ROOT because it is selected form the status bar + // ${workspaceFolder:} is not supported yet + "cmake.sourceDirectory": "${workspaceFolder}/CMakeLists.txt", + "cmake.installPrefix": "${workspaceFolder}/../../install", + "cmake.buildDirectory": "${workspaceFolder}/../../build", + "cmake.autoSelectActiveFolder": false, + "cmake.configureOnEdit": false, + // + // ROS + // + "ros.distro": "kinetic", + "cSpell.words": [ + "DCMAKE", + "rosparam" + ], + } +} \ No newline at end of file