search

IntelRealSense / librealsense

Intel® RealSense™ SDK
https://www.intelrealsense.com/
Apache License 2.0
7.63k stars 4.83k forks source link

d435 with ros2 #13468

Closed naorwaiss closed 5 days ago

naorwaiss commented 3 weeks ago

hi i try to run the d435 camera node with this command: ros2 launch realsense2_camera rs_launch.py depth_module.depth_profile:=1280x720x30 pointcloud.enable:=true

then i try to convert the point_cloud2 to laserscan to build a costmap

so i get 2 question: 1) i need some imu to build the cost_map ? - if i dont have internal imu i need to add one? 2) i try to use https://github.com/ros-perception/pointcloud_to_laserscan/tree/humble to convert from pointcloud to laserscan but it lock like i have alot of inf at my topic of the laserscan

thanks for the help!

MartyG-RealSense commented 3 weeks ago

Hi @naorwaiss Thanks very much for your questions. I researched your question carefully. A popular approach appears to be to use costmap_2d to generate the costmap after depthimage_to_laserscan has been used, as described at https://github.com/IntelRealSense/realsense-ros/issues/2442

To the best of my knowledge you should not need an IMU for producing the costmap.

naorwaiss commented 3 weeks ago

tanks for the help --- i stack at the convert from the depth to the laser scan but i fix it

naorwaiss commented 1 week ago

ok so i try for sometime to fix and understand some problem with laser scan and with the tf of the camera and the world but now i have problem with the nav2 

naor@naor-ASUS-TUF-Gaming-F15-FX507VV-FX507VV:~/Desktop/naor/study/hamama$ ros2 launch camera_nav camera_nav_launch.py 
[INFO] [launch]: All log files can be found below /home/naor/.ros/log/2024-11-08-23-00-54-466274-naor-ASUS-TUF-Gaming-F15-FX507VV-FX507VV-198704
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container_isolated-1]: process started with pid [198717]
[component_container_isolated-1] [INFO] [1731099655.213319178] [nav2_container]: Load Library: /opt/ros/humble/lib/libmap_server_core.so
[component_container_isolated-1] [INFO] [1731099655.241847206] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_map_server::CostmapFilterInfoServer>
[component_container_isolated-1] [INFO] [1731099655.241905363] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_map_server::MapSaver>
[component_container_isolated-1] [INFO] [1731099655.241911909] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_map_server::MapServer>
[component_container_isolated-1] [INFO] [1731099655.241916854] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_map_server::MapServer>
[component_container_isolated-1] [INFO] [1731099655.251611028] [map_server]: 
[component_container_isolated-1]        map_server lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.251903268] [map_server]: Creating
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/map_server' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.255103939] [nav2_container]: Load Library: /opt/ros/humble/lib/libamcl_core.so
[component_container_isolated-1] [INFO] [1731099655.261536136] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_amcl::AmclNode>
[component_container_isolated-1] [INFO] [1731099655.261574612] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_amcl::AmclNode>
[component_container_isolated-1] [INFO] [1731099655.266763365] [amcl]: 
[component_container_isolated-1]        amcl lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.267740829] [amcl]: Creating
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/amcl' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.271349913] [nav2_container]: Load Library: /opt/ros/humble/lib/libnav2_lifecycle_manager_core.so
[component_container_isolated-1] [INFO] [1731099655.272451772] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_lifecycle_manager::LifecycleManager>
[component_container_isolated-1] [INFO] [1731099655.272469229] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_lifecycle_manager::LifecycleManager>
[component_container_isolated-1] [INFO] [1731099655.276839180] [lifecycle_manager_localization]: Creating
[component_container_isolated-1] [INFO] [1731099655.279721954] [lifecycle_manager_localization]: Creating and initializing lifecycle service clients
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/lifecycle_manager_localization' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.282156822] [lifecycle_manager_localization]: Starting managed nodes bringup...
[component_container_isolated-1] [INFO] [1731099655.282200139] [lifecycle_manager_localization]: Configuring map_server
[component_container_isolated-1] [INFO] [1731099655.282341865] [map_server]: Configuring
[component_container_isolated-1] [ERROR] [1731099655.282691227] [map_server]: Caught exception in callback for transition 10
[component_container_isolated-1] [ERROR] [1731099655.282704669] [map_server]: Original error: parameter 'yaml_filename' is not initialized
[component_container_isolated-1] [WARN] [1731099655.282720050] [map_server]: Error occurred while doing error handling.
[component_container_isolated-1] [FATAL] [1731099655.282728823] [map_server]: Lifecycle node map_server does not have error state implemented
[component_container_isolated-1] [ERROR] [1731099655.282873341] [lifecycle_manager_localization]: Failed to change state for node: map_server
[component_container_isolated-1] [ERROR] [1731099655.282890807] [lifecycle_manager_localization]: Failed to bring up all requested nodes. Aborting bringup.
[component_container_isolated-1] [INFO] [1731099655.320133881] [nav2_container]: Load Library: /opt/ros/humble/lib/libcontroller_server_core.so
[component_container_isolated-1] [INFO] [1731099655.329688183] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_controller::ControllerServer>
[component_container_isolated-1] [INFO] [1731099655.329781270] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_controller::ControllerServer>
[component_container_isolated-1] [INFO] [1731099655.347630530] [controller_server]: 
[component_container_isolated-1]        controller_server lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.359601260] [controller_server]: Creating controller server
[component_container_isolated-1] [INFO] [1731099655.378049700] [local_costmap.local_costmap]: 
[component_container_isolated-1]        local_costmap lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.380849611] [local_costmap.local_costmap]: Creating Costmap
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/controller_server' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.387368282] [nav2_container]: Load Library: /opt/ros/humble/lib/libsmoother_server_core.so
[component_container_isolated-1] [INFO] [1731099655.391952464] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_smoother::SmootherServer>
[component_container_isolated-1] [INFO] [1731099655.392000810] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_smoother::SmootherServer>
[component_container_isolated-1] [INFO] [1731099655.404500128] [smoother_server]: 
[component_container_isolated-1]        smoother_server lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.406953293] [smoother_server]: Creating smoother server
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/smoother_server' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.411082273] [nav2_container]: Load Library: /opt/ros/humble/lib/libplanner_server_core.so
[component_container_isolated-1] [INFO] [1731099655.413526216] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_planner::PlannerServer>
[component_container_isolated-1] [INFO] [1731099655.413568820] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_planner::PlannerServer>
[component_container_isolated-1] [INFO] [1731099655.429202471] [planner_server]: 
[component_container_isolated-1]        planner_server lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.431901304] [planner_server]: Creating
[component_container_isolated-1] [INFO] [1731099655.441344899] [global_costmap.global_costmap]: 
[component_container_isolated-1]        global_costmap lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.441855243] [global_costmap.global_costmap]: Creating Costmap
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/planner_server' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.446234303] [nav2_container]: Load Library: /opt/ros/humble/lib/libbehavior_server_core.so
[component_container_isolated-1] [INFO] [1731099655.453629564] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<behavior_server::BehaviorServer>
[component_container_isolated-1] [INFO] [1731099655.453686438] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<behavior_server::BehaviorServer>
[component_container_isolated-1] [INFO] [1731099655.462325086] [behavior_server]: 
[component_container_isolated-1]        behavior_server lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/behavior_server' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.468347738] [nav2_container]: Load Library: /opt/ros/humble/lib/libbt_navigator_core.so
[component_container_isolated-1] [INFO] [1731099655.471916095] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_bt_navigator::BtNavigator>
[component_container_isolated-1] [INFO] [1731099655.471961930] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_bt_navigator::BtNavigator>
[component_container_isolated-1] [INFO] [1731099655.482004003] [bt_navigator]: 
[component_container_isolated-1]        bt_navigator lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.482055596] [bt_navigator]: Creating
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/bt_navigator' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.484374291] [nav2_container]: Load Library: /opt/ros/humble/lib/libwaypoint_follower_core.so
[component_container_isolated-1] [INFO] [1731099655.486001191] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_waypoint_follower::WaypointFollower>
[component_container_isolated-1] [INFO] [1731099655.486018972] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_waypoint_follower::WaypointFollower>
[component_container_isolated-1] [INFO] [1731099655.491722413] [waypoint_follower]: 
[component_container_isolated-1]        waypoint_follower lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.492734366] [waypoint_follower]: Creating
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/waypoint_follower' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.494409933] [nav2_container]: Load Library: /opt/ros/humble/lib/libvelocity_smoother_core.so
[component_container_isolated-1] [INFO] [1731099655.495671970] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_velocity_smoother::VelocitySmoother>
[component_container_isolated-1] [INFO] [1731099655.495685862] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_velocity_smoother::VelocitySmoother>
[component_container_isolated-1] [INFO] [1731099655.501825134] [velocity_smoother]: 
[component_container_isolated-1]        velocity_smoother lifecycle node launched. 
[component_container_isolated-1]        Waiting on external lifecycle transitions to activate
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/velocity_smoother' in container '/nav2_container'
[component_container_isolated-1]        See https://design.ros2.org/articles/node_lifecycle.html for more information.
[component_container_isolated-1] [INFO] [1731099655.503437907] [nav2_container]: Found class: rclcpp_components::NodeFactoryTemplate<nav2_lifecycle_manager::LifecycleManager>
[component_container_isolated-1] [INFO] [1731099655.503463969] [nav2_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nav2_lifecycle_manager::LifecycleManager>
[component_container_isolated-1] [INFO] [1731099655.509002250] [lifecycle_manager_navigation]: Creating
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/lifecycle_manager_navigation' in container '/nav2_container'
[component_container_isolated-1] [INFO] [1731099655.510306645] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
[component_container_isolated-1] [INFO] [1731099655.517051695] [lifecycle_manager_navigation]: Starting managed nodes bringup...
[component_container_isolated-1] [INFO] [1731099655.517107432] [lifecycle_manager_navigation]: Configuring controller_server
[component_container_isolated-1] [INFO] [1731099655.517263817] [controller_server]: Configuring controller interface
[component_container_isolated-1] [INFO] [1731099655.517490737] [controller_server]: getting goal checker plugins..
[component_container_isolated-1] [INFO] [1731099655.517833507] [controller_server]: Controller frequency set to 20.0000Hz
[component_container_isolated-1] [INFO] [1731099655.517900708] [local_costmap.local_costmap]: Configuring
[component_container_isolated-1] [INFO] [1731099655.521838930] [local_costmap.local_costmap]: Using plugin "static_layer"
[component_container_isolated-1] [INFO] [1731099655.527059257] [local_costmap.local_costmap]: Subscribing to the map topic (/map) with transient local durability
[component_container_isolated-1] [INFO] [1731099655.528325532] [local_costmap.local_costmap]: Initialized plugin "static_layer"
[component_container_isolated-1] [INFO] [1731099655.528351472] [local_costmap.local_costmap]: Using plugin "obstacle_layer"
[component_container_isolated-1] [INFO] [1731099655.529037732] [local_costmap.local_costmap]: Subscribed to Topics: 
[component_container_isolated-1] [INFO] [1731099655.529245123] [local_costmap.local_costmap]: Initialized plugin "obstacle_layer"
[component_container_isolated-1] [INFO] [1731099655.529252418] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[component_container_isolated-1] [INFO] [1731099655.529655155] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[component_container_isolated-1] [INFO] [1731099655.536610600] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[component_container_isolated-1] [INFO] [1731099655.537395163] [controller_server]: Created goal checker : goal_checker of type nav2_controller::SimpleGoalChecker
[component_container_isolated-1] [INFO] [1731099655.537670923] [controller_server]: Controller Server has goal_checker  goal checkers available.
[component_container_isolated-1] [INFO] [1731099655.541254942] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[component_container_isolated-1] [INFO] [1731099655.542231903] [controller_server]: Setting transform_tolerance to 0.100000
[component_container_isolated-1] [ERROR] [1731099655.555265269] [controller_server]: Couldn't load critics! Caught exception: No critics defined for FollowPath
[component_container_isolated-1] [ERROR] [1731099655.562092578] [controller_server]: Caught exception in callback for transition 10
[component_container_isolated-1] [ERROR] [1731099655.562137408] [controller_server]: Original error: No critics defined for FollowPath
[component_container_isolated-1] [WARN] [1731099655.562159747] [controller_server]: Error occurred while doing error handling.
[component_container_isolated-1] [FATAL] [1731099655.562176885] [controller_server]: Lifecycle node controller_server does not have error state implemented
[component_container_isolated-1] [ERROR] [1731099655.562466261] [lifecycle_manager_navigation]: Failed to change state for node: controller_server
[component_container_isolated-1] [ERROR] [1731099655.562515314] [lifecycle_manager_navigation]: Failed to bring up all requested nodes. Aborting bringup.

this is the problem ---

the yamel file:

# Global Costmap Parameters
global_costmap:
  ros__parameters:
    update_frequency: 5.0
    publish_frequency: 2.0
    global_frame: "map"
    robot_base_frame: "base_link"
    resolution: 0.05
    width: 10.0
    height: 10.0
    footprint: "[[0.2, 0.2], [-0.2, 0.2], [-0.2, -0.2], [0.2, -0.2]]"
    robot_radius: 0.15
    inflation_radius: 0.1
    plugins: ["camera_obstacle_layer", "inflation_layer", "obstacle_layer"]

    camera_obstacle_layer:
      plugin: "package_name::CameraObstacleLayer"
      enabled: true
      # Additional parameters such as topic name or specific thresholds

    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      enabled: true
      inflation_radius: 0.1

    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      enabled: true
      observation_sources: scan
      scan:
        topic: "/scan"
        expected_update_rate: 10.0  # Rate in Hz
        marking: true
        clearing: true
        max_obstacle_height: 2.0
        min_obstacle_height: 0.0

# Local Costmap Parameters
local_costmap:
  ros__parameters:
    update_frequency: 10.0
    publish_frequency: 5.0
    global_frame: "odom"
    robot_base_frame: "base_link"
    resolution: 0.05
    width: 10.0
    height: 10.0
    footprint: "[[0.2, 0.2], [-0.2, 0.2], [-0.2, -0.2], [0.2, -0.2]]"
    robot_radius: 0.15
    inflation_radius: 0.1
    plugins: ["camera_obstacle_layer", "inflation_layer", "obstacle_layer"]

    camera_obstacle_layer:
      plugin: "package_name::CameraObstacleLayer"
      enabled: true
      # Possibly different parameters or thresholds for local processing

    inflation_layer:
      plugin: "nav2_costmap_2d::InflationLayer"
      enabled: true
      inflation_radius: 0.1

    obstacle_layer:
      plugin: "nav2_costmap_2d::ObstacleLayer"
      enabled: true
      observation_sources: scan
      scan:
        topic: "/scan"
        expected_update_rate: 10.0
        marking: true
        clearing: true
        max_obstacle_height: 2.0
        min_obstacle_height: 0.0

and the launch file:

import os
import launch
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    pkg_nav2_bringup = FindPackageShare('nav2_bringup').find('nav2_bringup')
    bringup_launch_file = os.path.join(pkg_nav2_bringup, 'launch', 'bringup_launch.py')

    # Path to your costmap parameters YAML
    pkg_share = FindPackageShare('camera_nav').find('camera_nav')
    param_file = os.path.join(pkg_share, 'config', 'costmap_params.yaml')
    map_file = os.path.join(pkg_share, 'maps', 'map.yaml')  # Update the path to your map file

    return launch.LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(bringup_launch_file),
            launch_arguments={
                'params_file': param_file,
                'map': map_file
            }.items()
        )
    ])

if i need to go and ask at the nav2 fill free to say.. thanks !!

MartyG-RealSense commented 1 week ago

I do not have knowledge about nav2 unfortunately, so I certainly recommend asking at the navigation2 support forum at the link below. Thanks!

https://github.com/ros-navigation/navigation2/issues

MartyG-RealSense commented 1 week ago

Hi @naorwaiss Bearing in mind the recommendation in the comment above, do you require further assistance with this case please? Thanks!

naorwaiss commented 5 days ago

hi you can close ...

‫בתאריך יום ו׳, 15 בנוב׳ 2024 ב-15:59 מאת ‪MartyG-RealSense‬‏ <‪ @.***‬‏>:‬

Hi @naorwaiss https://github.com/naorwaiss Bearing in mind the recommendation in the comment above, do you require further assistance with this case please? Thanks!

— Reply to this email directly, view it on GitHub https://github.com/IntelRealSense/librealsense/issues/13468#issuecomment-2478904515, or unsubscribe https://github.com/notifications/unsubscribe-auth/A5HOZR3F6OBSUB2K73PAPUL2AX42JAVCNFSM6AAAAABQXAMZQGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDINZYHEYDINJRGU . You are receiving this because you were mentioned.Message ID: @.***>

MartyG-RealSense commented 5 days ago

Thanks very much for the update!