Robotics StackExchange | Archived questions

Trouble of getting Octomap in MoveIt tutorial

Hi,

I've been working on a perception pipeline tutorial in ROS Melodic on Ubuntu 18.04. Everything worked fine until I faced this issue; I can't see Octomap in rviz (which is supposed to be showing after launching the demo file as you can see from the above tutorial link). I used the raw code (I haven't changed anything) and I launched the demo using roslaunch moveit_tutorials obstacle_avoidance_demo.launch.

I realized that /move_group/filtered_cloud rostopic is not publishing anything and I guess that is why Octomap is not shown. In this tutorial, I noticed that world frame is not connected to any other frames in the tf tree (everything else is connected) but since Octomap and the sensor both relay on camera_rgb_optical_frame frame, I think it should be okay. I tried different combinations of frame names (e.g., odom, world) but none of them worked for me. I could've pulled an issue directly in MoveIt tutorial but the response there seems not fast enough and I strongly believe that it requires a simple fix to get things fine..

Hope someone could help me out. Thanks in advance!

Best,

--

The code I have is basically the same as the MoveIt tutorial but let me write down the necessary parts here for better understanding.

Firstly, the following is the console output after launching obstacle_avoidance_demo.launch:

SUMMARY
========

PARAMETERS
 * /joint_state_publisher/use_gui: False
 * /move_group/allow_trajectory_execution: True
 * /move_group/controller_list: [{'joints': ['pan...
 * /move_group/hand/planner_configs: ['SBLkConfigDefau...
 * /move_group/initial: [{'pose': 'ready'...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/max_range: 5.0
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_fake_contr...
 * /move_group/moveit_manage_controllers: True
 * /move_group/octomap_frame: camera_rgb_optica...
 * /move_group/octomap_resolution: 0.05
 * /move_group/panda_arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/panda_arm_hand/planner_configs: ['SBLkConfigDefau...
 * /move_group/planner_configs/BFMTkConfigDefault/balanced: 0
 * /move_group/planner_configs/BFMTkConfigDefault/cache_cc: 1
 * /move_group/planner_configs/BFMTkConfigDefault/extended_fmt: 1
 * /move_group/planner_configs/BFMTkConfigDefault/heuristics: 1
 * /move_group/planner_configs/BFMTkConfigDefault/nearest_k: 1
 * /move_group/planner_configs/BFMTkConfigDefault/num_samples: 1000
 * /move_group/planner_configs/BFMTkConfigDefault/optimality: 1
 * /move_group/planner_configs/BFMTkConfigDefault/radius_multiplier: 1.0
 * /move_group/planner_configs/BFMTkConfigDefault/type: geometric::BFMT
 * /move_group/planner_configs/BKPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/BKPIECEkConfigDefault/failed_expansion_score_factor: 0.5
 * /move_group/planner_configs/BKPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/BKPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/BiESTkConfigDefault/range: 0.0
 * /move_group/planner_configs/BiESTkConfigDefault/type: geometric::BiEST
 * /move_group/planner_configs/BiTRRTkConfigDefault/cost_threshold: 1e300
 * /move_group/planner_configs/BiTRRTkConfigDefault/frountier_node_ratio: 0.1
 * /move_group/planner_configs/BiTRRTkConfigDefault/frountier_threshold: 0.0
 * /move_group/planner_configs/BiTRRTkConfigDefault/init_temperature: 100
 * /move_group/planner_configs/BiTRRTkConfigDefault/range: 0.0
 * /move_group/planner_configs/BiTRRTkConfigDefault/temp_change_factor: 0.1
 * /move_group/planner_configs/BiTRRTkConfigDefault/type: geometric::BiTRRT
 * /move_group/planner_configs/ESTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/ESTkConfigDefault/range: 0.0
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/FMTkConfigDefault/cache_cc: 1
 * /move_group/planner_configs/FMTkConfigDefault/extended_fmt: 1
 * /move_group/planner_configs/FMTkConfigDefault/heuristics: 0
 * /move_group/planner_configs/FMTkConfigDefault/nearest_k: 1
 * /move_group/planner_configs/FMTkConfigDefault/num_samples: 1000
 * /move_group/planner_configs/FMTkConfigDefault/radius_multiplier: 1.1
 * /move_group/planner_configs/FMTkConfigDefault/type: geometric::FMT
 * /move_group/planner_configs/KPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/KPIECEkConfigDefault/failed_expansion_score_factor: 0.5
 * /move_group/planner_configs/KPIECEkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/KPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/KPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/border_fraction: 0.9
 * /move_group/planner_configs/LBKPIECEkConfigDefault/min_valid_path_fraction: 0.5
 * /move_group/planner_configs/LBKPIECEkConfigDefault/range: 0.0
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/LBTRRTkConfigDefault/epsilon: 0.4
 * /move_group/planner_configs/LBTRRTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/LBTRRTkConfigDefault/range: 0.0
 * /move_group/planner_configs/LBTRRTkConfigDefault/type: geometric::LBTRRT
 * /move_group/planner_configs/LazyPRMkConfigDefault/range: 0.0
 * /move_group/planner_configs/LazyPRMkConfigDefault/type: geometric::LazyPRM
 * /move_group/planner_configs/LazyPRMstarkConfigDefault/type: geometric::LazyPR...
 * /move_group/planner_configs/PDSTkConfigDefault/type: geometric::PDST
 * /move_group/planner_configs/PRMkConfigDefault/max_nearest_neighbors: 10
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/ProjESTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/ProjESTkConfigDefault/range: 0.0
 * /move_group/planner_configs/ProjESTkConfigDefault/type: geometric::ProjEST
 * /move_group/planner_configs/RRTConnectkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/RRTkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/delay_collision_checking: 1
 * /move_group/planner_configs/RRTstarkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/RRTstarkConfigDefault/range: 0.0
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/range: 0.0
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /move_group/planner_configs/SPARSkConfigDefault/dense_delta_fraction: 0.001
 * /move_group/planner_configs/SPARSkConfigDefault/max_failures: 1000
 * /move_group/planner_configs/SPARSkConfigDefault/sparse_delta_fraction: 0.25
 * /move_group/planner_configs/SPARSkConfigDefault/stretch_factor: 3.0
 * /move_group/planner_configs/SPARSkConfigDefault/type: geometric::SPARS
 * /move_group/planner_configs/SPARStwokConfigDefault/dense_delta_fraction: 0.001
 * /move_group/planner_configs/SPARStwokConfigDefault/max_failures: 5000
 * /move_group/planner_configs/SPARStwokConfigDefault/sparse_delta_fraction: 0.25
 * /move_group/planner_configs/SPARStwokConfigDefault/stretch_factor: 3.0
 * /move_group/planner_configs/SPARStwokConfigDefault/type: geometric::SPARStwo
 * /move_group/planner_configs/STRIDEkConfigDefault/degree: 16
 * /move_group/planner_configs/STRIDEkConfigDefault/estimated_dimension: 0.0
 * /move_group/planner_configs/STRIDEkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/STRIDEkConfigDefault/max_degree: 18
 * /move_group/planner_configs/STRIDEkConfigDefault/max_pts_per_leaf: 6
 * /move_group/planner_configs/STRIDEkConfigDefault/min_degree: 12
 * /move_group/planner_configs/STRIDEkConfigDefault/min_valid_path_fraction: 0.2
 * /move_group/planner_configs/STRIDEkConfigDefault/range: 0.0
 * /move_group/planner_configs/STRIDEkConfigDefault/type: geometric::STRIDE
 * /move_group/planner_configs/STRIDEkConfigDefault/use_projected_distance: 0
 * /move_group/planner_configs/TRRTkConfigDefault/frountierNodeRatio: 0.1
 * /move_group/planner_configs/TRRTkConfigDefault/frountier_threshold: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/goal_bias: 0.05
 * /move_group/planner_configs/TRRTkConfigDefault/init_temperature: 10e-6
 * /move_group/planner_configs/TRRTkConfigDefault/k_constant: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/max_states_failed: 10
 * /move_group/planner_configs/TRRTkConfigDefault/min_temperature: 10e-10
 * /move_group/planner_configs/TRRTkConfigDefault/range: 0.0
 * /move_group/planner_configs/TRRTkConfigDefault/temp_change_factor: 2.0
 * /move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT
 * /move_group/planner_configs/TrajOptDefault/type: geometric::TrajOpt
 * /move_group/planning_plugin: ompl_interface/OM...
 * /move_group/planning_scene_monitor/publish_geometry_updates: True
 * /move_group/planning_scene_monitor/publish_planning_scene: True
 * /move_group/planning_scene_monitor/publish_state_updates: True
 * /move_group/planning_scene_monitor/publish_transforms_updates: True
 * /move_group/request_adapters: default_planner_r...
 * /move_group/sensors: [{'point_subsampl...
 * /move_group/start_state_max_bounds_error: 0.1
 * /move_group/trajectory_execution/allowed_execution_duration_scaling: 1.2
 * /move_group/trajectory_execution/allowed_goal_duration_margin: 0.5
 * /move_group/trajectory_execution/allowed_start_tolerance: 0.01
 * /robot_description: <?xml version="1....
 * /robot_description_kinematics/panda_arm/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/panda_arm/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/panda_arm/kinematics_solver_timeout: 0.05
 * /robot_description_planning/joint_limits/panda_finger_joint1/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/panda_finger_joint1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_finger_joint1/max_acceleration: 0
 * /robot_description_planning/joint_limits/panda_finger_joint1/max_velocity: 0.1
 * /robot_description_planning/joint_limits/panda_finger_joint2/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/panda_finger_joint2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_finger_joint2/max_acceleration: 0
 * /robot_description_planning/joint_limits/panda_finger_joint2/max_velocity: 0.1
 * /robot_description_planning/joint_limits/panda_joint1/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/panda_joint1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_joint1/max_acceleration: 3.75
 * /robot_description_planning/joint_limits/panda_joint1/max_velocity: 2.175
 * /robot_description_planning/joint_limits/panda_joint2/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/panda_joint2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_joint2/max_acceleration: 1.875
 * /robot_description_planning/joint_limits/panda_joint2/max_velocity: 2.175
 * /robot_description_planning/joint_limits/panda_joint3/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/panda_joint3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_joint3/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/panda_joint3/max_velocity: 2.175
 * /robot_description_planning/joint_limits/panda_joint4/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/panda_joint4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_joint4/max_acceleration: 3.125
 * /robot_description_planning/joint_limits/panda_joint4/max_velocity: 2.175
 * /robot_description_planning/joint_limits/panda_joint5/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/panda_joint5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_joint5/max_acceleration: 3.75
 * /robot_description_planning/joint_limits/panda_joint5/max_velocity: 2.61
 * /robot_description_planning/joint_limits/panda_joint6/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/panda_joint6/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_joint6/max_acceleration: 5
 * /robot_description_planning/joint_limits/panda_joint6/max_velocity: 2.61
 * /robot_description_planning/joint_limits/panda_joint7/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/panda_joint7/has_velocity_limits: True
 * /robot_description_planning/joint_limits/panda_joint7/max_acceleration: 5
 * /robot_description_planning/joint_limits/panda_joint7/max_velocity: 2.61
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: melodic
 * /rosversion: 1.14.3
 * /rviz_yoon_2346_1631734231727053100/panda_arm/kinematics_solver: kdl_kinematics_pl...
 * /rviz_yoon_2346_1631734231727053100/panda_arm/kinematics_solver_search_resolution: 0.005
 * /rviz_yoon_2346_1631734231727053100/panda_arm/kinematics_solver_timeout: 0.05
 * /source_list: ['/move_group/fak...

NODES
  /
    joint_state_desired_publisher (topic_tools/relay)
    joint_state_publisher (joint_state_publisher/joint_state_publisher)
    move_group (moveit_ros_move_group/move_group)
    point_clouds (moveit_tutorials/bag_publisher_maintain_time)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    rviz_yoon_2346_1631734231727053100 (rviz/rviz)
    to_panda_base (tf2_ros/static_transform_publisher)
    to_temp_link (tf2_ros/static_transform_publisher)
    virtual_joint_broadcaster_1 (tf2_ros/static_transform_publisher)

auto-starting new master
process[master]: started with pid [2370]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 2e625e12-fbea-11e9-8a9c-94e6f70a839f
process[rosout-1]: started with pid [2387]
started core service [/rosout]
process[virtual_joint_broadcaster_1-2]: started with pid [2395]
process[joint_state_publisher-3]: started with pid [2399]
process[joint_state_desired_publisher-4]: started with pid [2409]
process[robot_state_publisher-5]: started with pid [2412]
process[move_group-6]: started with pid [2419]
process[rviz_yoon_2346_1631734231727053100-7]: started with pid [2420]
process[point_clouds-8]: started with pid [2436]
process[to_temp_link-9]: started with pid [2437]
process[to_panda_base-10]: started with pid [2442]
[ INFO] [1572531884.553191483]: Loading robot model 'panda'...
[ INFO] [1572531884.675087425]: rviz version 1.13.4
[ INFO] [1572531884.675207142]: compiled against Qt version 5.9.5
[ INFO] [1572531884.675237190]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1572531884.703547705]: Forcing OpenGl version 0.
[ INFO] [1572531884.776138332]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1572531884.779674158]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1572531884.779734736]: Starting planning scene monitor
[ INFO] [1572531884.782572441]: Listening to '/planning_scene'
[ INFO] [1572531884.782629265]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1572531884.785173825]: Listening to '/collision_object'
[ INFO] [1572531884.787750061]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1572531884.832635213]: Listening to '/camera/depth_registered/points' using message filter with target frame 'world '
[ INFO] [1572531884.844594451]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1572531884.906568605]: Initializing OMPL interface using ROS parameters
[ INFO] [1572531884.943376019]: Using planning interface 'OMPL'
[ INFO] [1572531884.948720340]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1572531884.949174321]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1572531884.949537202]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1572531884.949972654]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1572531884.950302220]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1572531884.950699098]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1572531884.950815714]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1572531884.950844903]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1572531884.950865975]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1572531884.950886708]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1572531884.950911533]: Using planning request adapter 'Resolve constraint frames to robot links'
[ INFO] [1572531884.950932503]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1572531884.981591224]: Loading robot model 'panda'...
[ INFO] [1572531884.986624109]: Stereo is NOT SUPPORTED
[ INFO] [1572531884.986778294]: OpenGl version: 3 (GLSL 1.3).
[ INFO] [1572531885.065713961]: Set joints of group 'panda_arm_hand' to pose 'ready'.
[ INFO] [1572531885.066205282]: Fake controller 'fake_panda_arm_controller' with joints [ panda_joint1 panda_joint2 panda_joint3 panda_joint4 panda_joint5 panda_joint6 panda_joint7 ]
[ INFO] [1572531885.066610498]: Fake controller 'fake_hand_controller' with joints [ panda_finger_joint1 panda_finger_joint2 ]
[ INFO] [1572531885.066972072]: Returned 2 controllers in list
[ INFO] [1572531885.077895218]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1572531885.180853674]: 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

[ INFO] [1572531885.180915989]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1572531885.180945579]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1572531885.271060059]: Loading robot model 'panda'...
[ WARN] [1572531885.537300042]: Unable to update multi-DOF joint 'virtual_joint': Failure to lookup transform between 'world' and 'panda_link0' with TF exception: Could not find a connection between 'world' and 'panda_link0' because they are not part of the same tree.Tf has two or more unconnected trees.
[ INFO] [1572531885.691647624]: Loading robot model 'panda'...
[ WARN] [1572531885.747243091]: Kinematics solver doesn't support #attempts anymore, but only a timeout.
Please remove the parameter '/rviz_yoon_2346_1631734231727053100/panda_arm/kinematics_solver_attempts' from your configuration.
[ INFO] [1572531885.813918660]: Starting planning scene monitor
[ INFO] [1572531885.819578421]: Listening to '/move_group/monitored_planning_scene'

obstacle_avoidance_demo.launch: 

  <!-- Play the rosbag that contains the pointcloud data -->
  <node pkg="moveit_tutorials" type="bag_publisher_maintain_time" name="point_clouds" />

  <!-- If needed, broadcast static tf for robot root -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="to_temp_link" args="0 0.4 -0.6 0 0 0  temp_link panda_link0" />
  <node pkg="tf2_ros" type="static_transform_publisher" name="to_panda_base" args="0 0 0 0 0.2 1.92 camera_rgb_optical_frame temp_link" />

</launch>

demo.launch file:

<launch>

  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" />
  <!-- Allow user to specify database location -->
  <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />

  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />
  <arg name="pipeline" default="ompl" />

  <!--
  By default, hide joint_state_publisher's GUI

  MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
  The latter one maintains and publishes the current joint configuration of the simulated robot.
  It also provides a GUI to move the simulated robot around "manually".
  This corresponds to moving around the real robot without the use of MoveIt.
  -->
  <arg name="rviz_tutorial" default="false" />
  <arg name="use_gui" default="false" />

  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
  </include>

  <!-- If needed, broadcast static tf for robot root -->
  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
  </node>
  <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find panda_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="pipeline" value="$(arg pipeline)"  />
  </include>

  <!-- Run Rviz -->
  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
    <arg name="rviz_tutorial" value="$(arg rviz_tutorial)"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  </include>

</launch>

move_group.launch:

<launch>

  <arg name="load_gripper" default="true" />

  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
    <arg name="load_gripper" value="$(arg load_gripper)" />
  </include>

  <arg name="pipeline" default="ompl" />

  <!-- GDB Debug Option -->
  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix"
       value="gdb -x $(find panda_moveit_config)/launch/gdb_settings.gdb --ex run --args" />

  <!-- Verbose Mode Option -->
  <arg name="info" default="$(arg debug)" />
  <arg unless="$(arg info)" name="command_args" value="" />
  <arg     if="$(arg info)" name="command_args" value="--debug" />

  <!-- move_group settings -->
  <arg name="allow_trajectory_execution" default="true"/>
  <arg name="fake_execution" default="false"/>
  <arg name="max_safe_path_cost" default="1"/>
  <arg name="jiggle_fraction" default="0.05" />
  <arg name="publish_monitored_planning_scene" default="true"/>

  <!-- Planning Functionality -->
  <include ns="move_group" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
    <arg name="pipeline" value="$(arg pipeline)" />
  </include>

  <!-- Trajectory Execution Functionality -->
  <include ns="move_group" file="$(find panda_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_manage_controllers" value="true" />
    <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
    <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
  </include>

  <!-- Sensors Functionality -->
  <include ns="move_group" file="$(find panda_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_sensor_manager" value="panda" />
  </include>

  <!-- Start the actual move_group node/action server -->
  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
    <!-- Set the display variable, in case OpenGL code is used internally -->
    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />

    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />

    <!-- load these non-default MoveGroup capabilities -->
    <!--
    <param name="capabilities" value="
                  a_package/AwsomeMotionPlanningCapability
                  another_package/GraspPlanningPipeline
                  " />
    -->

    <!-- inhibit these default MoveGroup capabilities -->
    <!--
    <param name="disable_capabilities" value="
                  move_group/MoveGroupKinematicsService
                  move_group/ClearOctomapService
                  " />
    -->

    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />

    <remap from="/joint_states" to="/joint_states_desired" />
  </node>

</launch>

sensor_manager.launch.xml:

<launch>

  <!-- This file makes it easy to include the settings for sensor managers -->

  <!-- Param for kinect config -->
  <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />

  <!-- Params for the octomap monitor -->
  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
  <param name="octomap_frame" type="string" value="camera_rgb_optical_frame" />
  <param name="octomap_resolution" type="double" value="0.05" />
  <param name="max_range" type="double" value="5.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="panda" />
  <include file="$(find panda_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />

</launch>

sensors_kinect_pointcloud.yaml:

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /camera/depth_registered/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

Asked by yoo00 on 2019-10-31 09:43:19 UTC

Comments

I meet the same problem. The frame settings have something wrong in tf2.

In obstacle_avoidance_demo.launch

<node pkg="tf2_ros" type="static_transform_publisher" name="to_temp_link" args="0 0.4 -0.6 0 0 0  temp_link panda_link0" />
<node pkg="tf2_ros" type="static_transform_publisher" name="to_panda_base" args="0 0 0 0 0.2 1.92 camera_rgb_optical_frame temp_link" />

These two lines are not correct, since tf2 removes multiple parent frames, and pand_link0 already has a parent world. You should inverse the parent-child relation:

<node pkg="tf2_ros" type="static_transform_publisher" name="to_temp_link" args="0 -0.4 0.6 0 0 0  panda_link0 temp_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="to_panda_base" args="0 0 0 0.1882209 0.0680271 -1.91357652 temp_link camera_rgb_optical_frame" />

But this just makes me see the right point clouds, I still cannot see the octomap

Asked by heinrich on 2020-02-10 22:39:38 UTC

After I changed obstacle_avoidance_demo.launch as you say:

<node pkg="tf2_ros" type="static_transform_publisher" name="to_temp_link" args="0 -0.4 0.6 0 0 0 panda_link0 temp_link"/> 
<node pkg="tf2_ros" type="static_transform_publisher" name="to_panda_base" args="0 0 0 0.1882209 0.0680271 -1.91357652 temp_link camera_rgb_optical_frame"/>

I can see the right octomap ! you are right !

Asked by liyou117 on 2020-04-09 17:19:36 UTC

@liyou117, please add your solution as an answer below and marked as solved.

Asked by askkvn on 2020-04-28 07:43:01 UTC

Answers