diff --git a/README.md b/README.md index fa679967e8..e02e7a22e5 100644 --- a/README.md +++ b/README.md @@ -23,48 +23,75 @@ Instructions on how to update the firmware can be found on the [Intel homepage]( # ROS Wrapper for Intel® RealSense™ Devices These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. -LibRealSense supported version: v2.31.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) +LibRealSense supported version: v2.35.2 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) ## Installation Instructions -The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kinetic, on **Ubuntu 16.04**. +The following instructions are written for ROS Kinetic, on **Ubuntu 16.04** but apply to ROS Melodic on **Ubuntu 18.04** as well, by replacing kinetic with melodic wherever is needed. -#### The simplest way to install on a clean machine is to follow the instructions on the [.travis.yml](https://github.com/intel-ros/realsense/blob/development/.travis.yml) file. It basically summerize the elaborate instructions in the following 3 steps: + ### Step 1: Install the ROS distribution + - #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 or [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) on Ubuntu 18.04. -### Step 1: Install the latest Intel® RealSense™ SDK 2.0 -- #### Install from [Debian Package](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) - In that case treat yourself as a developer. Make sure you follow the instructions to also install librealsense2-dev and librealsense-dkms packages. -#### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.31.0) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) +### There are 2 sources to install realsense2_camera from: -### Step 2: Install the ROS distribution -- #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 +* ### Method 1: The ROS distribution: + realsense2_camera is available as a debian package of ROS distribution. It can be installed by typing: + + ``` + export ROS_VER=kinetic + ``` + or + ``` + export ROS_VER=melodic + ``` + sudo apt-get install ros-$ROS_VER-realsense2-camera` -### Step 3: Install Intel® RealSense™ ROS from Sources -- Create a [catkin](http://wiki.ros.org/catkin#Installing_catkin) workspace -```bash -mkdir -p ~/catkin_ws/src -cd ~/catkin_ws/src/ -``` -- Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' -```bashrc -git clone https://github.com/IntelRealSense/realsense-ros.git -cd realsense-ros/ -git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1` -cd .. -``` -- Make sure all dependent packages are installed. You can check .travis.yml file for reference. -- Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.0) + This will install both realsense2_camera and its dependents, including librealsense2 library. -```bash -catkin_init_workspace -cd .. -catkin_make clean -catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release -catkin_make install -echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc -source ~/.bashrc -``` + Notice: + * The version of librealsense2 is almost always behind the one availeable in RealSense™ official repository. + * librealsense2 is not built to use native v4l2 driver but the less stable RS-USB protocol. That is because the last is more general and operational on a larger variety of platforms. + * any_realsense2_description is available as a separate debian package of ROS distribution. It includes the 3D-models of the devices and is necessary for running launch files that include these models (i.e. rs_d435_camera_with_model.launch). It can be installed by typing: + `sudo apt-get install ros-$ROS_VER-realsense2-description` + + +* ### Method 2: The RealSense™ distribution: + + #### This option is demonstrated in the [.travis.yml](https://github.com/intel-ros/realsense/blob/development/.travis.yml) file. It basically summerize the elaborate instructions in the following 2 steps: + + ### Step 1: Install the latest Intel® RealSense™ SDK 2.0 + - #### Install from [Debian Package](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) - In that case treat yourself as a developer. Make sure you follow the instructions to also install librealsense2-dev and librealsense-dkms packages. + + #### OR + - #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.35.2) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) + + + ### Step 2: Install Intel® RealSense™ ROS from Sources + - Create a [catkin](http://wiki.ros.org/catkin#Installing_catkin) workspace + ```bash + mkdir -p ~/catkin_ws/src + cd ~/catkin_ws/src/ + ``` + - Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' + ```bashrc + git clone https://github.com/IntelRealSense/realsense-ros.git + cd realsense-ros/ + git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1` + cd .. + ``` + - Make sure all dependent packages are installed. You can check .travis.yml file for reference. + - Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.2) + + ```bash + catkin_init_workspace + cd .. + catkin_make clean + catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release + catkin_make install + echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc + source ~/.bashrc + ``` ## Usage Instructions @@ -131,9 +158,9 @@ The topics are of the form: ```/camera/aligned_depth_to_color/image_raw``` etc. - **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. - **All the rest of the frame_ids can be found in the template launch file: [nodelet.launch.xml](./realsense2_camera/launch/includes/nodelet.launch.xml)** - **unite_imu_method**: The D435i and T265 cameras have built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. -Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. Under the new topic, all the fields in the Imu message are filled out. - - **linear_interpolation**: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. Items A and B are accel and gyro interchangeably, according to which type recently arrived from the sensor. The idea is to give the most recent information, united and without repetitions. - - **copy**: For each new message, accel or gyro, the relevant fields and timestamp are filled out while the others maintain the previous data. +Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. The *imu* topic is published at the rate of the gyro. All the fields of the Imu message under the *imu* topic are filled out. + - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. + - **copy**: Every gyro message is attached by the last accel message. - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. @@ -224,10 +251,10 @@ The wrapper publishes static transformations(TFs). The Frame Ids are divided int - base_link: For example: camera_link. A reference frame for the device. In D400 series and SR300 it is the depth frame. In T265, the pose frame. -### realsense2_description package: +### any_realsense2_description package: For viewing included models, a separate package is included. For example: ```bash -roslaunch realsense2_description view_d415_model.launch +roslaunch any_realsense2_description view_d415_model.launch ``` ### Unit tests: @@ -250,7 +277,6 @@ python src/realsense/realsense2_camera/scripts/rs2_test.py --all ## Known Issues * This ROS node does not currently support [ROS Lunar Loggerhead](http://wiki.ros.org/lunar). * This ROS node does not currently work with [ROS 2](https://github.com/ros2/ros2/wiki). -* This ROS node currently does not provide the unit-tests which ensure the proper operation of the camera. Future versions of the node will provide ROS compatible unit-tests. * This ROS node currently does not support running multiple T265 cameras at once. This will be addressed in a future update. ## License diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst new file mode 100644 index 0000000000..ed027199a2 --- /dev/null +++ b/realsense2_camera/CHANGELOG.rst @@ -0,0 +1,21 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package realsense2_camera +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.2.14 (2020-06-18) +------------------- +* Fix compatibility with Librealsense2 Version 2.35.2. +* Fix support for L515. +* Fix urdf issues. +* Add noetic support: change state_publisher into robot_state_publisher +* fix distortion correction model for T265 (equidistant) +* fix stability issues. Stop sensors at program termination. +* Contributors: Brice, Helen Oleynikova, doronhi + +* upgrade version to 2.2.13 +* fix ctrl-C closing issues. +* handle device creation exceptions. +* support LiDAR camera L515. +* optimize pointcloud. Contributors: Davide Faconti +* fix usb port id parsing issues. +* Add eigen dependency - missing for Melodic. Contributors: Antoine Hoarau diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 51fc701b85..5fc31b4a47 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -39,12 +39,17 @@ if(SET_USER_BREAK_AT_STARTUP) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG") endif() -# find_package(realsense2 2.29.0) -# if(NOT realsense2_FOUND) -# message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") -# endif() +find_package(realsense2 2.35.2) +if(NOT realsense2_FOUND) + message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") +endif() + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() -if (CMAKE_BUILD_TYPE EQUAL "RELEASE") +string(TOUPPER "${CMAKE_BUILD_TYPE}" uppercase_CMAKE_BUILD_TYPE) +if (${uppercase_CMAKE_BUILD_TYPE} STREQUAL "RELEASE") message(STATUS "Create Release Build.") set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}") else() @@ -71,8 +76,8 @@ generate_messages( set(CMAKE_NO_SYSTEM_FROM_IMPORTED true) include_directories( include - SYSTEM - ${catkin_INCLUDE_DIRS} + ${realsense2_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 7f3a1cbe5e..1c470a7888 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -428,6 +428,9 @@ namespace realsense2_camera stream_index_pair _base_stream; const std::string _namespace; + sensor_msgs::PointCloud2 _msg_pointcloud; + std::vector< unsigned int > _valid_pc_indices; + };//end class } diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 17a0282df5..caa9d6547a 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 11 +#define REALSENSE_ROS_PATCH_VERSION 14 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) @@ -34,6 +34,8 @@ namespace realsense2_camera const uint16_t RS435i_RGB_PID = 0x0B3A; // AWGC_MM const uint16_t RS405_PID = 0x0b0c; // DS5U const uint16_t RS_T265_PID = 0x0b37; // + const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // + const uint16_t RS_L515_PID = 0x0B64; // const bool ALIGN_DEPTH = false; diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 417327a619..6a69bc0925 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -29,6 +29,7 @@ namespace realsense2_camera { const stream_index_pair COLOR{RS2_STREAM_COLOR, 0}; const stream_index_pair DEPTH{RS2_STREAM_DEPTH, 0}; + const stream_index_pair INFRA0{RS2_STREAM_INFRARED, 0}; const stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1}; const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; const stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0}; @@ -39,7 +40,7 @@ namespace realsense2_camera const stream_index_pair POSE{RS2_STREAM_POSE, 0}; - const std::vector IMAGE_STREAMS = {DEPTH, INFRA1, INFRA2, + const std::vector IMAGE_STREAMS = {DEPTH, INFRA0, INFRA1, INFRA2, COLOR, FISHEYE, FISHEYE1, FISHEYE2}; @@ -67,6 +68,7 @@ namespace realsense2_camera void getDevice(rs2::device_list list); virtual void onInit() override; void tryGetLogSeverity(rs2_log_severity& severity) const; + static std::string parse_usb_port(std::string line); rs2::device _device; std::unique_ptr _realSenseNode; @@ -77,5 +79,6 @@ namespace realsense2_camera bool _initial_reset; std::thread _query_thread; + bool _is_alive; }; }//end namespace diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 1f0521e6b9..61b9010487 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ any_realsense2_camera - 2.2.11 + 2.2.14 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg @@ -15,6 +15,7 @@ Sergey Dorodnicov Doron Hirshberg catkin + eigen image_transport cv_bridge nav_msgs diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 37b16f25ef..176ea09723 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -131,7 +131,7 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, BaseRealSenseNode::~BaseRealSenseNode() { // Kill dynamic transform thread - if (_publish_tf && _tf_publish_rate > 0) + if (_tf_t) _tf_t->join(); _is_running = false; @@ -140,6 +140,18 @@ BaseRealSenseNode::~BaseRealSenseNode() { _monitoring_t->join(); } + + std::set module_names; + for (const std::pair>& profile : _enabled_profiles) + { + std::string module_name = _sensors[profile.first].get_info(RS2_CAMERA_INFO_NAME); + std::pair< std::set::iterator, bool> res = module_names.insert(module_name); + if (res.second) + { + _sensors[profile.first].stop(); + _sensors[profile.first].close(); + } + } } void BaseRealSenseNode::toggleSensors(bool enabled) @@ -167,9 +179,8 @@ void BaseRealSenseNode::setupErrorCallback() { s.set_notifications_callback([&](const rs2::notification& n) { - std::vector error_strings({"RT IC2 Config error", - "Motion Module force pause", - "stream start failure"}); + std::vector error_strings({"RT IC2 Config error", + "Left IC2 Config error"}); if (n.get_severity() >= RS2_LOG_SEVERITY_ERROR) { ROS_WARN_STREAM("Hardware Notification:" << n.get_description() << "," << n.get_timestamp() << "," << n.get_severity() << "," << n.get_category()); @@ -177,7 +188,7 @@ void BaseRealSenseNode::setupErrorCallback() if (error_strings.end() != find_if(error_strings.begin(), error_strings.end(), [&n] (std::string err) {return (n.get_description().find(err) != std::string::npos); })) { - ROS_ERROR_STREAM("Hardware Reset is needed. use option: initial_reset:=true"); + ROS_ERROR_STREAM("Performing Hardware Reset."); _dev.hardware_reset(); } }); @@ -239,14 +250,18 @@ bool is_checkbox(rs2::options sensor, rs2_option option) bool is_enum_option(rs2::options sensor, rs2_option option) { + static const int MAX_ENUM_OPTION_VALUES(100); + static const float EPSILON(0.05); + rs2::option_range op_range = sensor.get_option_range(option); - if (op_range.step < 0.001f) return false; + if (abs((op_range.step - 1)) > EPSILON || (op_range.max > MAX_ENUM_OPTION_VALUES)) return false; for (auto i = op_range.min; i <= op_range.max; i += op_range.step) { if (sensor.get_option_value_description(option, i) == nullptr) - return false; + continue; + return true; } - return true; + return false; } bool is_int_option(rs2::options sensor, rs2_option option) @@ -266,6 +281,8 @@ std::map get_enum_method(rs2::options sensor, rs2_option optio const auto op_range_step = int(op_range.step); for (auto val = op_range_min; val <= op_range_max; val += op_range_step) { + if (sensor.get_option_value_description(option, val) == nullptr) + continue; dict[sensor.get_option_value_description(option, val)] = val; } } @@ -740,46 +757,36 @@ void BaseRealSenseNode::setupDevice() std::function multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame, _imu_sync_method);}; ROS_DEBUG_STREAM("Device Sensors: "); - for(auto&& elem : _dev_sensors) + for(auto&& sensor : _dev_sensors) { - std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME); - - if ("Stereo Module" == module_name) + for (auto& profile : sensor.get_stream_profiles()) { - _sensors[DEPTH] = elem; - _sensors[INFRA1] = elem; - _sensors[INFRA2] = elem; - _sensors_callback[module_name] = frame_callback_function; + auto video_profile = profile.as(); + stream_index_pair sip(video_profile.stream_type(), video_profile.stream_index()); + if (_sensors.find( sip ) != _sensors.end()) + continue; + _sensors[sip] = sensor; } - else if ("Coded-Light Depth Sensor" == module_name) + + std::string module_name = sensor.get_info(RS2_CAMERA_INFO_NAME); + if (sensor.is()) { - _sensors[DEPTH] = elem; - _sensors[INFRA1] = elem; _sensors_callback[module_name] = frame_callback_function; } - else if ("RGB Camera" == module_name) + else if (sensor.is()) { - _sensors[COLOR] = elem; _sensors_callback[module_name] = frame_callback_function; } - else if ("Wide FOV Camera" == module_name) + else if (sensor.is()) { - _sensors[FISHEYE] = elem; _sensors_callback[module_name] = frame_callback_function; } - else if ("Motion Module" == module_name) + else if (sensor.is()) { - _sensors[GYRO] = elem; - _sensors[ACCEL] = elem; _sensors_callback[module_name] = imu_callback_function; } - else if ("Tracking Module" == module_name) + else if (sensor.is()) { - _sensors[GYRO] = elem; - _sensors[ACCEL] = elem; - _sensors[POSE] = elem; - _sensors[FISHEYE1] = elem; - _sensors[FISHEYE2] = elem; _sensors_callback[module_name] = multiple_message_callback_function; } else @@ -1105,7 +1112,7 @@ void BaseRealSenseNode::setupFilters() if (use_decimation_filter) { ROS_DEBUG("Add Filter: decimation"); - _filters.insert(_filters.begin(),NamedFilter("decimation", std::make_shared())); + _filters.insert(_filters.begin(),NamedFilter("decimation", std::make_shared())); } if (use_colorizer_filter) { @@ -1967,7 +1974,12 @@ void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& v _camera_info[stream_index].P.at(10) = 1; _camera_info[stream_index].P.at(11) = 0; - _camera_info[stream_index].distortion_model = "plumb_bob"; + if (intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4) + { + _camera_info[stream_index].distortion_model = "equidistant"; + } else { + _camera_info[stream_index].distortion_model = "plumb_bob"; + } // set R (rotation matrix) values to identity matrix _camera_info[stream_index].R.at(0) = 1.0; @@ -2056,7 +2068,7 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const stream_index_pair& s { if (!strcmp(e.what(), "Requested extrinsics are not available!")) { - ROS_WARN_STREAM(e.what() << " : using unity as default."); + ROS_WARN_STREAM("(" << rs2_stream_to_string(stream.first) << ", " << stream.second << ") -> (" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << "): " << e.what() << " : using unity as default."); ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); } else @@ -2236,7 +2248,8 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co const rs2::vertex* vertex = pc.get_vertices(); const rs2::texture_coordinate* color_point = pc.get_texture_coordinates(); - std::list valid_indices; + + _valid_pc_indices.clear(); for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, color_point++) { if (static_cast(vertex->z) > 0) @@ -2245,19 +2258,18 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co float j = static_cast(color_point->v); if (_allow_no_texture_points || (i >= 0.f && i <= 1.f && j >= 0.f && j <= 1.f)) { - valid_indices.push_back(point_idx); + _valid_pc_indices.push_back(point_idx); } } } - sensor_msgs::PointCloud2 msg_pointcloud; - msg_pointcloud.header.stamp = t; - msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH]; - msg_pointcloud.width = valid_indices.size(); - msg_pointcloud.height = 1; - msg_pointcloud.is_dense = true; + _msg_pointcloud.header.stamp = t; + _msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH]; + _msg_pointcloud.width = _valid_pc_indices.size(); + _msg_pointcloud.height = 1; + _msg_pointcloud.is_dense = true; - sensor_msgs::PointCloud2Modifier modifier(msg_pointcloud); + sensor_msgs::PointCloud2Modifier modifier(_msg_pointcloud); modifier.setPointCloud2FieldsByString(1, "xyz"); vertex = pc.get_vertices(); @@ -2280,19 +2292,19 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co default: throw std::runtime_error("Unhandled texture format passed in pointcloud " + std::to_string(texture_frame.get_profile().format())); } - msg_pointcloud.point_step = addPointField(msg_pointcloud, format_str.c_str(), 1, sensor_msgs::PointField::FLOAT32, msg_pointcloud.point_step); - msg_pointcloud.row_step = msg_pointcloud.width * msg_pointcloud.point_step; - msg_pointcloud.data.resize(msg_pointcloud.height * msg_pointcloud.row_step); + _msg_pointcloud.point_step = addPointField(_msg_pointcloud, format_str.c_str(), 1, sensor_msgs::PointField::FLOAT32, _msg_pointcloud.point_step); + _msg_pointcloud.row_step = _msg_pointcloud.width * _msg_pointcloud.point_step; + _msg_pointcloud.data.resize(_msg_pointcloud.height * _msg_pointcloud.row_step); - sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); - sensor_msgs::PointCloud2Iteratoriter_color(msg_pointcloud, format_str); + sensor_msgs::PointCloud2Iteratoriter_x(_msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(_msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(_msg_pointcloud, "z"); + sensor_msgs::PointCloud2Iteratoriter_color(_msg_pointcloud, format_str); color_point = pc.get_texture_coordinates(); float color_pixel[2]; unsigned int prev_idx(0); - for (auto idx=valid_indices.begin(); idx != valid_indices.end(); idx++) + for (auto idx=_valid_pc_indices.begin(); idx != _valid_pc_indices.end(); idx++) { unsigned int idx_jump(*idx-prev_idx); prev_idx = *idx; @@ -2321,11 +2333,11 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co } else { - sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); + sensor_msgs::PointCloud2Iteratoriter_x(_msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(_msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(_msg_pointcloud, "z"); unsigned int prev_idx(0); - for (auto idx=valid_indices.begin(); idx != valid_indices.end(); idx++) + for (auto idx=_valid_pc_indices.begin(); idx != _valid_pc_indices.end(); idx++) { unsigned int idx_jump(*idx-prev_idx); prev_idx = *idx; @@ -2338,7 +2350,7 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co ++iter_x; ++iter_y; ++iter_z; } } - _pointcloud_publisher.publish(msg_pointcloud); + _pointcloud_publisher.publish(_msg_pointcloud); } diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 73552be0a4..174235b0f5 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -21,7 +21,8 @@ constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR PLUGINLIB_EXPORT_CLASS(realsense2_camera::RealSenseNodeFactory, nodelet::Nodelet) -RealSenseNodeFactory::RealSenseNodeFactory() +RealSenseNodeFactory::RealSenseNodeFactory(): + _is_alive(true) { ROS_INFO_STREAM("Running with LibRealSense v" << RS2_API_VERSION_STR << " and RealSense ROS v" << REALSENSE_ROS_VERSION_STR); @@ -33,18 +34,35 @@ RealSenseNodeFactory::RealSenseNodeFactory() rs2::log_to_console(severity); } -void RealSenseNodeFactory::closeDevice() +RealSenseNodeFactory::~RealSenseNodeFactory() { - for(rs2::sensor sensor : _device.query_sensors()) + _is_alive = false; + if (_query_thread.joinable()) { - sensor.stop(); - sensor.close(); + _query_thread.join(); } } -RealSenseNodeFactory::~RealSenseNodeFactory() +std::string RealSenseNodeFactory::parse_usb_port(std::string line) { - closeDevice(); + std::string port_id; + std::regex self_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*", std::regex_constants::ECMAScript); + std::smatch base_match; + bool found = std::regex_match(line, base_match, self_regex); + if (found) + { + port_id = base_match[1].str(); + if (base_match[2].str().size() == 0) //This is libuvc string. Remove counter is exists. + { + std::regex end_regex = std::regex(".+(-[0-9]+$)", std::regex_constants::ECMAScript); + bool found_end = std::regex_match(port_id, base_match, end_regex); + if (found_end) + { + port_id = port_id.substr(0, port_id.size() - base_match[1].str().size()); + } + } + } + return port_id; } void RealSenseNodeFactory::getDevice(rs2::device_list list) @@ -58,40 +76,28 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) else { bool found = false; - // ROS_INFO_STREAM(" "); - for (auto&& dev : list) + ROS_INFO_STREAM(" "); + for (size_t count = 0; count < list.size(); count++) { - auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."< results; - ROS_DEBUG_STREAM("Device with name " << name << " was found."); - std::regex self_regex; - if(name == std::string("Intel RealSense T265")) - { - self_regex = std::regex(".*?bus_([0-9]+) port_([0-9]+).*", std::regex_constants::ECMAScript); - } - else// if(strcmp(name, "Intel RealSense D435") == 0) + rs2::device dev; + try { - self_regex = std::regex("[^ ]+/usb[0-9]+[0-9./-]*/([0-9.-]+):[^ ]*", std::regex_constants::ECMAScript); + dev = list[count]; } - std::smatch base_match; - bool found_usb_desc = std::regex_match(pn, base_match, self_regex); - if (found_usb_desc) + catch(const std::exception& ex) { - std::ssub_match base_sub_match = base_match[1]; - port_id = base_sub_match.str(); - for (unsigned int mi = 2; mi < base_match.size(); mi++) - { - std::ssub_match base_sub_match = base_match[mi]; - port_id += "-" + base_sub_match.str(); - } - ROS_DEBUG_STREAM("Device with port number " << port_id << " was found."); + ROS_WARN_STREAM("Device " << count+1 << "/" << list.size() << " failed with exception: " << ex.what()); + continue; } - else + auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); + ROS_INFO_STREAM("Device with serial number " << sn << " was found."< results; + ROS_INFO_STREAM("Device with name " << name << " was found."); + std::string port_id = parse_usb_port(pn); + if (port_id.empty()) { std::stringstream msg; msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros"; @@ -105,11 +111,16 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) ROS_ERROR_STREAM("Please use serial number instead of usb port."); } } + else + { + ROS_INFO_STREAM("Device with port number " << port_id << " was found."); + } bool found_device_type(true); if (!_device_type.empty()) { + std::smatch match_results; std::regex device_type_regex(_device_type.c_str(), std::regex::icase); - found_device_type = std::regex_search(name, base_match, device_type_regex); + found_device_type = std::regex_search(name, match_results, device_type_regex); } if ((_serial_no.empty() || sn == _serial_no) && (_usb_port_id.empty() || port_id == _usb_port_id) && found_device_type) @@ -189,7 +200,7 @@ void RealSenseNodeFactory::change_device_callback(rs2::event_information& info) rs2::device_list new_devices = info.get_new_devices(); if (new_devices.size() > 0) { - ROS_INFO("Searching for new devices..."); + ROS_INFO("Checking new devices..."); getDevice(new_devices); if (_device) { @@ -227,7 +238,6 @@ void RealSenseNodeFactory::onInit() pipe->start(cfg); //File will be opened in read mode at this point _device = pipe->get_active_profile().get_device(); _serial_no = _device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); } if (_device) { @@ -241,9 +251,8 @@ void RealSenseNodeFactory::onInit() _query_thread = std::thread([=]() { std::chrono::milliseconds timespan(6000); - while (!_device) + while (_is_alive && !_device) { - // _ctx.init_tracking_module(); // Unavailable function. getDevice(_ctx.query_devices()); if (_device) { @@ -273,6 +282,7 @@ void RealSenseNodeFactory::onInit() void RealSenseNodeFactory::StartDevice() { + if (_realSenseNode) _realSenseNode.reset(); ros::NodeHandle nh = getNodeHandle(); ros::NodeHandle privateNh = getPrivateNodeHandle(); // TODO @@ -295,6 +305,7 @@ void RealSenseNodeFactory::StartDevice() case RS435_RGB_PID: case RS435i_RGB_PID: case RS_USB2_PID: + case RS_L515_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); break; case RS_T265_PID: diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst new file mode 100644 index 0000000000..32fcfbae00 --- /dev/null +++ b/realsense2_description/CHANGELOG.rst @@ -0,0 +1,14 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package any_realsense2_description +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.2.14 (2020-06-18) +------------------- +* fix urdf issues (arg use_nominal_extrinsics). +* Add noetic support: + - urdf files. + - change state_publisher into robot_state_publisher +* correct offset between camera_link and base_link +* Contributors: Brice, Marco Camurri, doronhi + +* upgrade version to 2.2.13 diff --git a/realsense2_description/CMakeLists.txt b/realsense2_description/CMakeLists.txt index 78d270f9f6..2eb7007351 100644 --- a/realsense2_description/CMakeLists.txt +++ b/realsense2_description/CMakeLists.txt @@ -1,6 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) project(any_realsense2_description) - find_package(catkin REQUIRED COMPONENTS ) @@ -15,4 +14,4 @@ install(DIRECTORY launch meshes rviz urdf # Tests if (CATKIN_ENABLE_TESTING) catkin_add_nosetests(tests) -endif() \ No newline at end of file +endif() diff --git a/realsense2_description/launch/view_multiple_d435_models.launch b/realsense2_description/launch/view_multiple_d435_models.launch index 1999b73b90..e687996781 100644 --- a/realsense2_description/launch/view_multiple_d435_models.launch +++ b/realsense2_description/launch/view_multiple_d435_models.launch @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/launch/view_r410_model.launch b/realsense2_description/launch/view_r410_model.launch index 336c67f503..186bead271 100644 --- a/realsense2_description/launch/view_r410_model.launch +++ b/realsense2_description/launch/view_r410_model.launch @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/launch/view_r430_model.launch b/realsense2_description/launch/view_r430_model.launch index aa1f79b930..1572cc2d74 100644 --- a/realsense2_description/launch/view_r430_model.launch +++ b/realsense2_description/launch/view_r430_model.launch @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index c7f89dd0c6..0d9aeb5753 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ any_realsense2_description - 2.2.11 + 2.2.14 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg @@ -15,4 +15,4 @@ catkin xacro rosunit - + \ No newline at end of file diff --git a/realsense2_description/tests/dual_d415.xacro b/realsense2_description/tests/dual_d415.xacro index 373de30f23..a2c5870fb2 100644 --- a/realsense2_description/tests/dual_d415.xacro +++ b/realsense2_description/tests/dual_d415.xacro @@ -1,13 +1,13 @@ - + - + - - + + - + diff --git a/realsense2_description/tests/dual_d435.xacro b/realsense2_description/tests/dual_d435.xacro index de798a1c5d..d062875249 100644 --- a/realsense2_description/tests/dual_d435.xacro +++ b/realsense2_description/tests/dual_d435.xacro @@ -1,13 +1,13 @@ - + - + - - + + - + diff --git a/realsense2_description/tests/dual_r410.xacro b/realsense2_description/tests/dual_r410.xacro index 420d92cc02..686c52a5e3 100644 --- a/realsense2_description/tests/dual_r410.xacro +++ b/realsense2_description/tests/dual_r410.xacro @@ -1,13 +1,13 @@ - + - + - - + + - + diff --git a/realsense2_description/tests/dual_r430.xacro b/realsense2_description/tests/dual_r430.xacro index 8fbf872b1c..463e76149c 100644 --- a/realsense2_description/tests/dual_r430.xacro +++ b/realsense2_description/tests/dual_r430.xacro @@ -1,13 +1,13 @@ - + - + - - + + - + diff --git a/realsense2_description/tests/one_of_each.xacro b/realsense2_description/tests/one_of_each.xacro index 45b33a65e7..58c1bdf016 100644 --- a/realsense2_description/tests/one_of_each.xacro +++ b/realsense2_description/tests/one_of_each.xacro @@ -1,5 +1,5 @@ - + @@ -7,16 +7,16 @@ - + - - + + - - + + - - + + - + diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index 2a0c3aed5e..c50f5c1ec5 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -73,7 +73,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index 41dcbdb6dc..66e5dff971 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -28,6 +28,12 @@ aluminum peripherial evaluation case. + + + + + + @@ -44,14 +50,15 @@ aluminum peripherial evaluation case. - + - + + @@ -72,7 +79,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index 8a2bb05c3b..aefcad4b56 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -65,7 +65,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index a307a38718..3b2fec5bad 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -66,7 +66,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/test_d415_camera.urdf.xacro b/realsense2_description/urdf/test_d415_camera.urdf.xacro index ab7cf3f208..6a0d398122 100644 --- a/realsense2_description/urdf/test_d415_camera.urdf.xacro +++ b/realsense2_description/urdf/test_d415_camera.urdf.xacro @@ -1,9 +1,10 @@ - + + - + - + diff --git a/realsense2_description/urdf/test_d435_camera.urdf.xacro b/realsense2_description/urdf/test_d435_camera.urdf.xacro index 971648f303..4eb18b1f76 100644 --- a/realsense2_description/urdf/test_d435_camera.urdf.xacro +++ b/realsense2_description/urdf/test_d435_camera.urdf.xacro @@ -1,9 +1,10 @@ - + + - + - + diff --git a/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro index 072697cf44..f06a372426 100644 --- a/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro +++ b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro @@ -1,13 +1,14 @@ - + + - + - + - + - + diff --git a/realsense2_description/urdf/test_r410_camera.urdf.xacro b/realsense2_description/urdf/test_r410_camera.urdf.xacro index f1882ea60b..4feca48b22 100644 --- a/realsense2_description/urdf/test_r410_camera.urdf.xacro +++ b/realsense2_description/urdf/test_r410_camera.urdf.xacro @@ -1,9 +1,10 @@ - + + - + - + diff --git a/realsense2_description/urdf/test_r430_camera.urdf.xacro b/realsense2_description/urdf/test_r430_camera.urdf.xacro index 2541fca5ab..4f0e8c9d5a 100644 --- a/realsense2_description/urdf/test_r430_camera.urdf.xacro +++ b/realsense2_description/urdf/test_r430_camera.urdf.xacro @@ -1,9 +1,10 @@ - + + - + - +