remove unsupported models

README.md

@@ -200,13 +200,13 @@
   - They have, at least, the **profile** parameter.
     - The profile parameter is a string of the following format: \<width>X\<height>X\<fps> (The dividing character can be X, x or ",". Spaces are ignored.)
     - For example: ```depth_module.profile:=640x480x30 rgb_camera.profile:=1280x720x30```
-    - Since infra, infra1, infra2, fisheye, fisheye1, fisheye2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile**
+    - Since infra, infra1, infra2 and depth are all streams of the depth_module, their width, height and fps are defined by the same param **depth_module.profile**
     - If the specified combination of parameters is not available by the device, the default or previously set configuration will be used.
       - Run ```ros2 param describe <your_node_name> <param_name>``` to get the list of supported profiles.
     - Note: Should re-enable the stream for the change to take effect.
   - ***<stream_name>*_format**
     - This parameter is a string used to select the stream format.
-    - <stream_name> can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2*.
+    - <stream_name> can be any of *infra, infra1, infra2, color, depth*.
     - For example: ```depth_module.depth_format:=Z16 depth_module.infra1_format:=y8 rgb_camera.color_format:=RGB8```
     - This parameter supports both lower case and upper case letters.
     - If the specified parameter is not available by the stream, the default or previously set configuration will be used.
@@ -217,14 +217,14 @@
     - Run ```rs-enumerate-devices``` command to know the list of profiles supported by the connected sensors.
 - **enable_*<stream_name>***: 
   - Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams.
-  - <stream_name> can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*.
+  - <stream_name> can be any of *infra, infra1, infra2, color, depth, gyro, accel*.
   - For example: ```enable_infra1:=true enable_color:=false```
 - **enable_sync**:
   - gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag.
   - This happens automatically when such filters as pointcloud are enabled.
 - ***<stream_type>*_qos**: 
   - Sets the QoS by which the topic is published.
-  - <stream_type> can be any of *infra, infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose*.
+  - <stream_type> can be any of *infra, infra1, infra2, color, depth, gyro, accel*.
   -  Available values are the following strings: `SYSTEM_DEFAULT`, `DEFAULT`, `PARAMETER_EVENTS`, `SERVICES_DEFAULT`, `PARAMETERS`, `SENSOR_DATA`.
   - For example: ```depth_qos:=SENSOR_DATA```
   - Reference: [ROS2 QoS profiles formal documentation](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-profiles)
@@ -270,7 +270,6 @@
   - For example: `initial_reset:=true`
 - ***<stream_name>*_frame_id**, ***<stream_name>*_optical_frame_id**, **aligned_depth_to_*<stream_name>*_frame_id**: Specify the different frame_id for the different frames. Especially important when using multiple cameras.
 - **base_frame_id**: defines the frame_id all static transformations refers to.
-- **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.
 
 - **unite_imu_method**:
   - D400 cameras have built in IMU components which produce 2 unrelated streams, each with it's own frequency: 
@@ -299,8 +298,6 @@
   - 0 or negative values mean no diagnostics topic is published. Defaults to 0.</br>
 The `/diagnostics` topic includes information regarding the device temperatures and actual frequency of the enabled streams.
 
-- **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame.
-
 <hr>
 
 <h3 id="coordination">

realsense2_camera/include/base_realsense_node.h

@@ -40,7 +40,6 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
-#include <nav_msgs/msg/odometry.hpp>
 
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_ros/transform_broadcaster.h>
@@ -249,7 +248,6 @@ namespace realsense2_camera
         void FillImuData_LinearInterpolation(const CimuData imu_data, std::deque<sensor_msgs::msg::Imu>& imu_msgs);
         void imu_callback(rs2::frame frame);
         void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY);
-        void pose_callback(rs2::frame frame);
         void multiple_message_callback(rs2::frame frame, imu_sync_method sync_method);
         void frame_callback(rs2::frame frame);
 
@@ -295,7 +293,6 @@ namespace realsense2_camera
         std::map<stream_index_pair, std::shared_ptr<image_publisher>> _image_publishers;
 
         std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> _imu_publishers;
-        std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> _odom_publisher;
         std::shared_ptr<SyncedImuPublisher> _synced_imu_publisher;
         std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> _info_publishers;
         std::map<stream_index_pair, rclcpp::Publisher<realsense2_camera_msgs::msg::Metadata>::SharedPtr> _metadata_publishers;
@@ -316,7 +313,6 @@ namespace realsense2_camera
         bool _is_color_enabled;
         bool _is_depth_enabled;
         bool _pointcloud;
-        bool _publish_odom_tf;
         imu_sync_method _imu_sync_method;
         stream_index_pair _pointcloud_texture;
         PipelineSyncer _syncer;

realsense2_camera/include/constants.h

@@ -59,8 +59,6 @@
 
 namespace realsense2_camera
 {
-    const uint16_t SR300_PID        = 0x0aa5; // SR300
-    const uint16_t SR300v2_PID      = 0x0B48; // SR305
     const uint16_t RS400_PID        = 0x0ad1; // PSR
     const uint16_t RS410_PID        = 0x0ad2; // ASR
     const uint16_t RS415_PID        = 0x0ad3; // ASRC
@@ -80,11 +78,7 @@ namespace realsense2_camera
     const uint16_t RS430i_PID       = 0x0b4b; // D430i
     const uint16_t RS405_PID        = 0x0B5B; // DS5U
     const uint16_t RS455_PID        = 0x0B5C; // D455
-    const uint16_t RS457_PID        = 0xABCD; // D457
-    const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // 
-    const uint16_t RS_L515_PID      = 0x0B64; // 
-    const uint16_t RS_L535_PID      = 0x0b68;
-
+    const uint16_t RS457_PID        = 0xABCD; // D457   
 
     const bool ALLOW_NO_TEXTURE_POINTS = false;
     const bool ORDERED_PC     = false;
@@ -100,15 +94,10 @@ namespace realsense2_camera
     const std::string HID_QOS         = "SENSOR_DATA";
 
     const bool HOLD_BACK_IMU_FOR_FRAMES = false;
-    const bool PUBLISH_ODOM_TF = true;
 
     const std::string DEFAULT_BASE_FRAME_ID            = "link";
-    const std::string DEFAULT_ODOM_FRAME_ID            = "odom_frame";
     const std::string DEFAULT_IMU_OPTICAL_FRAME_ID     = "camera_imu_optical_frame";
 
-    const std::string DEFAULT_UNITE_IMU_METHOD         = "";
-    const std::string DEFAULT_FILTERS                  = "";
-
     const float ROS_DEPTH_SCALE = 0.001;
 
     static const rmw_qos_profile_t rmw_qos_profile_latched =

realsense2_camera/include/ros_utils.h

@@ -31,12 +31,8 @@ namespace realsense2_camera
     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};
-    const stream_index_pair FISHEYE1{RS2_STREAM_FISHEYE, 1};
-    const stream_index_pair FISHEYE2{RS2_STREAM_FISHEYE, 2};
     const stream_index_pair GYRO{RS2_STREAM_GYRO, 0};
     const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0};
-    const stream_index_pair POSE{RS2_STREAM_POSE, 0};
 
     bool isValidCharInName(char c);
 

realsense2_camera/launch/rs_launch.py

@@ -51,8 +51,6 @@
                            {'name': 'accel_fps',                    'default': '0', 'description': "''"},
                            {'name': 'enable_gyro',                  'default': 'false', 'description': "'enable gyro stream'"},
                            {'name': 'enable_accel',                 'default': 'false', 'description': "'enable accel stream'"},
-                           {'name': 'enable_pose',                  'default': 'true', 'description': "'enable pose stream'"},
-                           {'name': 'pose_fps',                     'default': '200', 'description': "''"},
                            {'name': 'pointcloud.enable',            'default': 'false', 'description': ''},
                            {'name': 'pointcloud.stream_filter',     'default': '2', 'description': 'texture stream for pointcloud'},
                            {'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'},

realsense2_camera/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
   <name>realsense2_camera</name>
   <version>4.54.1</version>
-  <description>RealSense camera package allowing access to Intel SR300 and D400 3D cameras</description>
+  <description>RealSense camera package allowing access to Intel D400 3D cameras</description>
   <maintainer email="librs.ros@intel.com">LibRealSense ROS Team</maintainer>
   <license>Apache License 2.0</license>
 

realsense2_camera/src/base_realsense_node.cpp

@@ -113,7 +113,6 @@ BaseRealSenseNode::BaseRealSenseNode(rclcpp::Node& node,
     _is_color_enabled(false),
     _is_depth_enabled(false),
     _pointcloud(false),
-    _publish_odom_tf(false),
     _imu_sync_method(imu_sync_method::NONE),
     _is_profile_changed(false),
     _is_align_depth_changed(false)
@@ -630,9 +629,6 @@ void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_met
             if (sync_method > imu_sync_method::NONE) imu_callback_sync(frame, sync_method);
             else imu_callback(frame);
             break;
-        case RS2_STREAM_POSE:
-            pose_callback(frame);
-            break;
         default:
             frame_callback(frame);
     }
@@ -788,7 +784,7 @@ void BaseRealSenseNode::updateExtrinsicsCalibData(const rs2::video_stream_profil
 
 void BaseRealSenseNode::SetBaseStream()
 {
-    const std::vector<stream_index_pair> base_stream_priority = {DEPTH, POSE};
+    const std::vector<stream_index_pair> base_stream_priority = {DEPTH};
     std::set<stream_index_pair> checked_sips;
     std::map<stream_index_pair, rs2::stream_profile> available_profiles;
     for(auto&& sensor : _available_ros_sensors)

realsense2_camera/src/parameters.cpp

@@ -77,10 +77,6 @@ void BaseRealSenseNode::getParameters()
     _hold_back_imu_for_frames = _parameters->setParam<bool>(param_name, HOLD_BACK_IMU_FOR_FRAMES);
     _parameters_names.push_back(param_name);
 
-    param_name = std::string("publish_odom_tf");
-    _publish_odom_tf = _parameters->setParam<bool>(param_name, PUBLISH_ODOM_TF);
-    _parameters_names.push_back(param_name);
-
     param_name = std::string("base_frame_id");
     _base_frame_id = _parameters->setParam<std::string>(param_name, DEFAULT_BASE_FRAME_ID);
     _base_frame_id = (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str();

realsense2_camera/src/realsense_node_factory.cpp

@@ -358,8 +358,6 @@ void RealSenseNodeFactory::startDevice()
     {
         switch(pid)
         {
-        case SR300_PID:
-        case SR300v2_PID:
         case RS400_PID:
         case RS405_PID:
         case RS410_PID:
@@ -377,9 +375,6 @@ void RealSenseNodeFactory::startDevice()
         case RS457_PID:
         case RS465_PID:
         case RS_USB2_PID:
-        case RS_L515_PID_PRE_PRQ:
-        case RS_L515_PID:
-        case RS_L535_PID:
             _realSenseNode = std::unique_ptr<BaseRealSenseNode>(new BaseRealSenseNode(*this, _device, _parameters, this->get_node_options().use_intra_process_comms()));
             break;
         default:

realsense2_camera/src/rs_node_setup.cpp

@@ -281,13 +281,6 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
             IMUInfo info_msg = getImuInfo(profile);
             _imu_info_publishers[sip]->publish(info_msg);
         }
-        else if (profile.is<rs2::pose_stream_profile>())
-        {
-            std::stringstream data_topic_name, info_topic_name;
-            data_topic_name << stream_name << "/sample";
-            _odom_publisher = _node.create_publisher<nav_msgs::msg::Odometry>(data_topic_name.str(),
-                rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos));
-        }
         std::string topic_metadata(stream_name + "/metadata");
         _metadata_publishers[sip] = _node.create_publisher<realsense2_camera_msgs::msg::Metadata>(topic_metadata, 
             rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(info_qos), info_qos));

realsense2_camera/src/tfs.cpp

@@ -309,91 +309,3 @@ void BaseRealSenseNode::startDynamicTf()
         }
     }
 }
-
-void BaseRealSenseNode::pose_callback(rs2::frame frame)
-{
-    double frame_time = frame.get_timestamp();
-    bool placeholder_false(false);
-    if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) )
-    {
-        _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain());
-    }
-
-    ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s",
-                rs2_stream_to_string(frame.get_profile().stream_type()),
-                frame.get_profile().stream_index(),
-                rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain()));
-    rs2_pose pose = frame.as<rs2::pose_frame>().get_pose_data();
-    rclcpp::Time t(frameSystemTimeSec(frame));
-
-    geometry_msgs::msg::PoseStamped pose_msg;
-    pose_msg.pose.position.x = -pose.translation.z;
-    pose_msg.pose.position.y = -pose.translation.x;
-    pose_msg.pose.position.z = pose.translation.y;
-    pose_msg.pose.orientation.x = -pose.rotation.z;
-    pose_msg.pose.orientation.y = -pose.rotation.x;
-    pose_msg.pose.orientation.z = pose.rotation.y;
-    pose_msg.pose.orientation.w = pose.rotation.w;
-
-    static tf2_ros::TransformBroadcaster br(_node);
-    geometry_msgs::msg::TransformStamped msg;
-    msg.header.stamp = t;
-    msg.header.frame_id = DEFAULT_ODOM_FRAME_ID;
-    msg.child_frame_id = FRAME_ID(POSE);
-    msg.transform.translation.x = pose_msg.pose.position.x;
-    msg.transform.translation.y = pose_msg.pose.position.y;
-    msg.transform.translation.z = pose_msg.pose.position.z;
-    msg.transform.rotation.x = pose_msg.pose.orientation.x;
-    msg.transform.rotation.y = pose_msg.pose.orientation.y;
-    msg.transform.rotation.z = pose_msg.pose.orientation.z;
-    msg.transform.rotation.w = pose_msg.pose.orientation.w;
-
-    if (_publish_odom_tf) br.sendTransform(msg);
-
-    if (0 != _odom_publisher->get_subscription_count())
-    {
-        double cov_pose(_linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence));
-        double cov_twist(_angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence));
-
-        geometry_msgs::msg::Vector3Stamped v_msg;
-        tf2::Vector3 tfv(-pose.velocity.z, -pose.velocity.x, pose.velocity.y);
-        tf2::Quaternion q(-msg.transform.rotation.x,
-                          -msg.transform.rotation.y,
-                          -msg.transform.rotation.z,
-                           msg.transform.rotation.w);
-        tfv=tf2::quatRotate(q,tfv);
-        v_msg.vector.x = tfv.x();
-        v_msg.vector.y = tfv.y();
-        v_msg.vector.z = tfv.z();
-
-        tfv = tf2::Vector3(-pose.angular_velocity.z, -pose.angular_velocity.x, pose.angular_velocity.y);
-        tfv=tf2::quatRotate(q,tfv);
-        geometry_msgs::msg::Vector3Stamped om_msg;
-        om_msg.vector.x = tfv.x();
-        om_msg.vector.y = tfv.y();
-        om_msg.vector.z = tfv.z();    
-
-        nav_msgs::msg::Odometry odom_msg;
-
-        odom_msg.header.frame_id = DEFAULT_ODOM_FRAME_ID;
-        odom_msg.child_frame_id = FRAME_ID(POSE);
-        odom_msg.header.stamp = t;
-        odom_msg.pose.pose = pose_msg.pose;
-        odom_msg.pose.covariance = {cov_pose, 0, 0, 0, 0, 0,
-                                    0, cov_pose, 0, 0, 0, 0,
-                                    0, 0, cov_pose, 0, 0, 0,
-                                    0, 0, 0, cov_twist, 0, 0,
-                                    0, 0, 0, 0, cov_twist, 0,
-                                    0, 0, 0, 0, 0, cov_twist};
-        odom_msg.twist.twist.linear = v_msg.vector;
-        odom_msg.twist.twist.angular = om_msg.vector;
-        odom_msg.twist.covariance ={cov_pose, 0, 0, 0, 0, 0,
-                                    0, cov_pose, 0, 0, 0, 0,
-                                    0, 0, cov_pose, 0, 0, 0,
-                                    0, 0, 0, cov_twist, 0, 0,
-                                    0, 0, 0, 0, cov_twist, 0,
-                                    0, 0, 0, 0, 0, cov_twist};
-        _odom_publisher->publish(odom_msg);
-        ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type()));
-    }
-}