get_current_pcd from camera.py returns empty list (again)

Jendker commented 3 years ago

I am running into the same issue with get_current_pcd as before in another issue: https://github.com/facebookresearch/pyrobot/issues/139 after I have updated Develop branch to the latest version and installed python script, compiled catkin_ws. I have also tested the master branch but it returns empty array as well. Were there any changes to get_current_pcd?

Steps to reproduce

install pyrobot with either python2 or python3 on locobot
run roslaunch locobot_control main.launch use_arm:=true use_camera:=true use_base:=true use_rviz:=false
get_current_pcd function returns empty list if executed in script (script below)

Observed Results

I have reinstalled the robot because of problem with the software and now the function get_current_pcd (https://github.com/facebookresearch/pyrobot/blob/master/src/pyrobot/locobot/camera.py#L68) returns empty array. rostopic /camera/depth_registered/points publishes the pointcloud fine, though. Verified with Rviz.

Expected Results

Before the software reinstall the pointcloud was returned correctly.

Relevant Code

from pyrobot import Robot

bot = Robot('locobot')
print('pcl', bot.camera.get_current_pcd(True))  # or False, does not matter

Jekyll1021 commented 3 years ago

Hi @Jendker could you print depth at https://github.com/facebookresearch/pyrobot/blob/4b3f0135f2610646e89840329cd1a2db3db5ee63/src/pyrobot/locobot/camera.py#L446

I think it might be a different problem this time. I checked the camera related files and they didn't seem to change, so I suspect there might be some problem upstream.

Jendker commented 3 years ago

Thank you for you answer! While I am still able to see the pointcloud in RViz on /camera/depth_registered/points topic, depth is all zeros. If that matters, we have not updated any packages on the robot in the meantime.

Jekyll1021 commented 3 years ago

Could you confirm the entire depth array is zero (unlike #139 where you get some numerically small number that get filtered out downstream)? If so, is there any error / warning in the roslaunch window?

I ran a fresh install over here and fetched a depth image with value. Typically the outer-rim of a depth image can be zeros with a few values in between.

Jendker commented 3 years ago

I can confirm that entire depth array is zero and there are no errors in the roslaunch window:

roslaunch locobot_control main.launch use_arm:=true use_camera:=true use_base:=true use_rviz:=false
... logging to /home/locobot/.ros/log/3c4860e8-adc9-11eb-900b-48f17fded622/roslaunch-locobot-19738.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://locobot:39827/

SUMMARY
========

PARAMETERS
 * /bumper2pointcloud/pointcloud_radius: 0.24
 * /camera/realsense2_camera/accel_fps: 250
 * /camera/realsense2_camera/accel_frame_id: camera_accel_frame
 * /camera/realsense2_camera/accel_optical_frame_id: camera_accel_opti...
 * /camera/realsense2_camera/align_depth: True
 * /camera/realsense2_camera/aligned_depth_to_color_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_fisheye_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra1_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/aligned_depth_to_infra2_frame_id: camera_aligned_de...
 * /camera/realsense2_camera/allow_no_texture_points: False
 * /camera/realsense2_camera/base_frame_id: camera_link
 * /camera/realsense2_camera/calib_odom_file: 
 * /camera/realsense2_camera/clip_distance: -1.0
 * /camera/realsense2_camera/color_fps: 30
 * /camera/realsense2_camera/color_frame_id: camera_color_frame
 * /camera/realsense2_camera/color_height: 480
 * /camera/realsense2_camera/color_optical_frame_id: camera_color_opti...
 * /camera/realsense2_camera/color_width: 640
 * /camera/realsense2_camera/depth_fps: 30
 * /camera/realsense2_camera/depth_frame_id: camera_depth_frame
 * /camera/realsense2_camera/depth_height: 480
 * /camera/realsense2_camera/depth_optical_frame_id: camera_depth_opti...
 * /camera/realsense2_camera/depth_width: 640
 * /camera/realsense2_camera/device_type: 
 * /camera/realsense2_camera/enable_accel: True
 * /camera/realsense2_camera/enable_color: True
 * /camera/realsense2_camera/enable_depth: True
 * /camera/realsense2_camera/enable_fisheye1: True
 * /camera/realsense2_camera/enable_fisheye2: True
 * /camera/realsense2_camera/enable_fisheye: True
 * /camera/realsense2_camera/enable_gyro: True
 * /camera/realsense2_camera/enable_infra1: True
 * /camera/realsense2_camera/enable_infra2: True
 * /camera/realsense2_camera/enable_pointcloud: False
 * /camera/realsense2_camera/enable_sync: True
 * /camera/realsense2_camera/filters: 
 * /camera/realsense2_camera/fisheye1_frame_id: camera_fisheye1_f...
 * /camera/realsense2_camera/fisheye1_optical_frame_id: camera_fisheye1_o...
 * /camera/realsense2_camera/fisheye2_frame_id: camera_fisheye2_f...
 * /camera/realsense2_camera/fisheye2_optical_frame_id: camera_fisheye2_o...
 * /camera/realsense2_camera/fisheye_fps: 30
 * /camera/realsense2_camera/fisheye_frame_id: camera_fisheye_frame
 * /camera/realsense2_camera/fisheye_height: 480
 * /camera/realsense2_camera/fisheye_optical_frame_id: camera_fisheye_op...
 * /camera/realsense2_camera/fisheye_width: 640
 * /camera/realsense2_camera/gyro_fps: 400
 * /camera/realsense2_camera/gyro_frame_id: camera_gyro_frame
 * /camera/realsense2_camera/gyro_optical_frame_id: camera_gyro_optic...
 * /camera/realsense2_camera/imu_optical_frame_id: camera_imu_optica...
 * /camera/realsense2_camera/infra1_frame_id: camera_infra1_frame
 * /camera/realsense2_camera/infra1_optical_frame_id: camera_infra1_opt...
 * /camera/realsense2_camera/infra2_frame_id: camera_infra2_frame
 * /camera/realsense2_camera/infra2_optical_frame_id: camera_infra2_opt...
 * /camera/realsense2_camera/infra_fps: 30
 * /camera/realsense2_camera/infra_height: 480
 * /camera/realsense2_camera/infra_width: 640
 * /camera/realsense2_camera/initial_reset: False
 * /camera/realsense2_camera/json_file_path: 
 * /camera/realsense2_camera/linear_accel_cov: 0.01
 * /camera/realsense2_camera/odom_frame_id: camera_odom_frame
 * /camera/realsense2_camera/pointcloud_texture_index: 0
 * /camera/realsense2_camera/pointcloud_texture_stream: RS2_STREAM_COLOR
 * /camera/realsense2_camera/pose_frame_id: camera_pose_frame
 * /camera/realsense2_camera/pose_optical_frame_id: camera_pose_optic...
 * /camera/realsense2_camera/publish_odom_tf: True
 * /camera/realsense2_camera/publish_tf: True
 * /camera/realsense2_camera/rosbag_filename: 
 * /camera/realsense2_camera/serial_no: 
 * /camera/realsense2_camera/tf_publish_rate: 0.0
 * /camera/realsense2_camera/topic_odom_in: camera/odom_in
 * /camera/realsense2_camera/unite_imu_method: none
 * /camera/realsense2_camera/usb_port_id: 
 * /cmd_vel_mux/yaml_cfg_file: /home/locobot/low...
 * /diagnostic_aggregator/analyzers/input_ports/contains: ['Digital Input',...
 * /diagnostic_aggregator/analyzers/input_ports/path: Input Ports
 * /diagnostic_aggregator/analyzers/input_ports/remove_prefix: mobile_base_nodel...
 * /diagnostic_aggregator/analyzers/input_ports/timeout: 5.0
 * /diagnostic_aggregator/analyzers/input_ports/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/kobuki/contains: ['Watchdog', 'Mot...
 * /diagnostic_aggregator/analyzers/kobuki/path: Kobuki
 * /diagnostic_aggregator/analyzers/kobuki/remove_prefix: mobile_base_nodel...
 * /diagnostic_aggregator/analyzers/kobuki/timeout: 5.0
 * /diagnostic_aggregator/analyzers/kobuki/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/power/contains: ['Battery', 'Lapt...
 * /diagnostic_aggregator/analyzers/power/path: Power System
 * /diagnostic_aggregator/analyzers/power/remove_prefix: mobile_base_nodel...
 * /diagnostic_aggregator/analyzers/power/timeout: 5.0
 * /diagnostic_aggregator/analyzers/power/type: diagnostic_aggreg...
 * /diagnostic_aggregator/analyzers/sensors/contains: ['Cliff Sensor', ...
 * /diagnostic_aggregator/analyzers/sensors/path: Sensors
 * /diagnostic_aggregator/analyzers/sensors/remove_prefix: mobile_base_nodel...
 * /diagnostic_aggregator/analyzers/sensors/timeout: 5.0
 * /diagnostic_aggregator/analyzers/sensors/type: diagnostic_aggreg...
 * /diagnostic_aggregator/base_path: 
 * /diagnostic_aggregator/pub_rate: 1.0
 * /dynamixel_info: /home/locobot/low...
 * /locobot_controller/dxl_read_period: 0.01
 * /locobot_controller/dxl_write_period: 0.01
 * /locobot_controller/publish_period: 0.01
 * /mobile_base/base_frame: base_footprint
 * /mobile_base/battery_capacity: 16.5
 * /mobile_base/battery_dangerous: 13.2
 * /mobile_base/battery_low: 14.0
 * /mobile_base/cmd_vel_timeout: 0.6
 * /mobile_base/device_port: /dev/kobuki
 * /mobile_base/odom_frame: odom
 * /mobile_base/publish_tf: True
 * /mobile_base/use_imu_heading: True
 * /mobile_base/wheel_left_joint_name: wheel_left_joint
 * /mobile_base/wheel_right_joint_name: wheel_right_joint
 * /move_base/DWAPlannerROS/acc_lim_theta: 2.0
 * /move_base/DWAPlannerROS/acc_lim_x: 1.0
 * /move_base/DWAPlannerROS/acc_lim_y: 0.0
 * /move_base/DWAPlannerROS/forward_point_distance: 0.325
 * /move_base/DWAPlannerROS/global_frame_id: odom
 * /move_base/DWAPlannerROS/goal_distance_bias: 24.0
 * /move_base/DWAPlannerROS/latch_xy_goal_tolerance: True
 * /move_base/DWAPlannerROS/max_rot_vel: 2.0
 * /move_base/DWAPlannerROS/max_scaling_factor: 0.2
 * /move_base/DWAPlannerROS/max_trans_vel: 0.1
 * /move_base/DWAPlannerROS/max_vel_x: 0.15
 * /move_base/DWAPlannerROS/max_vel_y: 0.0
 * /move_base/DWAPlannerROS/min_rot_vel: 1.0
 * /move_base/DWAPlannerROS/min_trans_vel: 0.1
 * /move_base/DWAPlannerROS/min_vel_x: 0.1
 * /move_base/DWAPlannerROS/min_vel_y: 0.0
 * /move_base/DWAPlannerROS/occdist_scale: 0.5
 * /move_base/DWAPlannerROS/oscillation_reset_dist: 0.05
 * /move_base/DWAPlannerROS/path_distance_bias: 64.0
 * /move_base/DWAPlannerROS/publish_cost_grid_pc: True
 * /move_base/DWAPlannerROS/publish_traj_pc: True
 * /move_base/DWAPlannerROS/rot_stopped_vel: 0.5
 * /move_base/DWAPlannerROS/scaling_speed: 0.25
 * /move_base/DWAPlannerROS/sim_time: 1.0
 * /move_base/DWAPlannerROS/stop_time_buffer: 0.2
 * /move_base/DWAPlannerROS/trans_stopped_vel: 0.1
 * /move_base/DWAPlannerROS/vtheta_samples: 20
 * /move_base/DWAPlannerROS/vx_samples: 20
 * /move_base/DWAPlannerROS/vy_samples: 1
 * /move_base/DWAPlannerROS/xy_goal_tolerance: 0.15
 * /move_base/DWAPlannerROS/yaw_goal_tolerance: 0.15
 * /move_base/GlobalPlanner/allow_unknown: True
 * /move_base/GlobalPlanner/cost_factor: 3.0
 * /move_base/GlobalPlanner/default_tolerance: 0.0
 * /move_base/GlobalPlanner/lethal_cost: 253
 * /move_base/GlobalPlanner/neutral_cost: 50
 * /move_base/GlobalPlanner/old_navfn_behavior: False
 * /move_base/GlobalPlanner/planner_costmap_publish_frequency: 0.0
 * /move_base/GlobalPlanner/planner_window_x: 0.0
 * /move_base/GlobalPlanner/planner_window_y: 0.0
 * /move_base/GlobalPlanner/publish_potential: True
 * /move_base/GlobalPlanner/publish_scale: 100
 * /move_base/GlobalPlanner/use_dijkstra: True
 * /move_base/GlobalPlanner/use_grid_path: False
 * /move_base/GlobalPlanner/use_quadratic: True
 * /move_base/NavfnROS/allow_unknown: False
 * /move_base/NavfnROS/default_tolerance: 0.0
 * /move_base/NavfnROS/planner_window_x: 0.0
 * /move_base/NavfnROS/planner_window_y: 0.0
 * /move_base/NavfnROS/visualize_potential: False
 * /move_base/TrajectoryPlannerROS/acc_lim_theta: 1.0
 * /move_base/TrajectoryPlannerROS/acc_lim_x: 0.3
 * /move_base/TrajectoryPlannerROS/acc_lim_y: 0.0
 * /move_base/TrajectoryPlannerROS/angular_sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/dwa: True
 * /move_base/TrajectoryPlannerROS/escape_vel: -0.15
 * /move_base/TrajectoryPlannerROS/gdist_scale: 0.7
 * /move_base/TrajectoryPlannerROS/heading_lookahead: 0.375
 * /move_base/TrajectoryPlannerROS/heading_scoring: False
 * /move_base/TrajectoryPlannerROS/heading_scoring_timestep: 0.8
 * /move_base/TrajectoryPlannerROS/holonomic_robot: False
 * /move_base/TrajectoryPlannerROS/latch_xy_goal_tolerance: True
 * /move_base/TrajectoryPlannerROS/max_vel_theta: 0.7
 * /move_base/TrajectoryPlannerROS/max_vel_x: 0.36
 * /move_base/TrajectoryPlannerROS/max_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/meter_scoring: True
 * /move_base/TrajectoryPlannerROS/min_in_place_vel_theta: 0.5
 * /move_base/TrajectoryPlannerROS/min_vel_theta: -0.7
 * /move_base/TrajectoryPlannerROS/min_vel_x: 0.2
 * /move_base/TrajectoryPlannerROS/min_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/occdist_scale: 0.01
 * /move_base/TrajectoryPlannerROS/oscillation_reset_dist: 0.05
 * /move_base/TrajectoryPlannerROS/pdist_scale: 0.6
 * /move_base/TrajectoryPlannerROS/prune_plan: True
 * /move_base/TrajectoryPlannerROS/publish_cost_grid_pc: False
 * /move_base/TrajectoryPlannerROS/sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/simple_attractor: False
 * /move_base/TrajectoryPlannerROS/vtheta_samples: 40
 * /move_base/TrajectoryPlannerROS/vx_samples: 28
 * /move_base/TrajectoryPlannerROS/vy_samples: 0
 * /move_base/TrajectoryPlannerROS/xy_goal_tolerance: 0.2
 * /move_base/TrajectoryPlannerROS/yaw_goal_tolerance: 0.2
 * /move_base/base_global_planner: global_planner/Gl...
 * /move_base/base_local_planner: base_local_planne...
 * /move_base/controller_frequency: 5.0
 * /move_base/controller_patience: 1.0
 * /move_base/global_costmap/always_send_full_costmap: True
 * /move_base/global_costmap/global_frame: map
 * /move_base/global_costmap/height: 100.0
 * /move_base/global_costmap/inflation_layer/cost_scaling_factor: 5.0
 * /move_base/global_costmap/inflation_layer/enabled: True
 * /move_base/global_costmap/inflation_layer/inflation_radius: 0.5
 * /move_base/global_costmap/map_type: costmap
 * /move_base/global_costmap/max_obstacle_height: 0.6
 * /move_base/global_costmap/obstacle_layer/bump/clearing: False
 * /move_base/global_costmap/obstacle_layer/bump/data_type: PointCloud2
 * /move_base/global_costmap/obstacle_layer/bump/marking: True
 * /move_base/global_costmap/obstacle_layer/bump/max_obstacle_height: 0.15
 * /move_base/global_costmap/obstacle_layer/bump/min_obstacle_height: 0.0
 * /move_base/global_costmap/obstacle_layer/bump/topic: mobile_base/senso...
 * /move_base/global_costmap/obstacle_layer/combination_method: 1
 * /move_base/global_costmap/obstacle_layer/enabled: True
 * /move_base/global_costmap/obstacle_layer/mark_threshold: 0
 * /move_base/global_costmap/obstacle_layer/max_obstacle_height: 0.6
 * /move_base/global_costmap/obstacle_layer/observation_sources: scan bump
 * /move_base/global_costmap/obstacle_layer/obstacle_range: 2.5
 * /move_base/global_costmap/obstacle_layer/origin_z: 0.0
 * /move_base/global_costmap/obstacle_layer/publish_voxel_map: False
 * /move_base/global_costmap/obstacle_layer/raytrace_range: 3.0
 * /move_base/global_costmap/obstacle_layer/scan/clearing: True
 * /move_base/global_costmap/obstacle_layer/scan/data_type: LaserScan
 * /move_base/global_costmap/obstacle_layer/scan/marking: True
 * /move_base/global_costmap/obstacle_layer/scan/max_obstacle_height: 0.35
 * /move_base/global_costmap/obstacle_layer/scan/min_obstacle_height: 0.25
 * /move_base/global_costmap/obstacle_layer/scan/topic: scan
 * /move_base/global_costmap/obstacle_layer/track_unknown_space: True
 * /move_base/global_costmap/obstacle_layer/unknown_threshold: 15
 * /move_base/global_costmap/obstacle_layer/z_resolution: 0.2
 * /move_base/global_costmap/obstacle_layer/z_voxels: 2
 * /move_base/global_costmap/plugins: [{'type': 'costma...
 * /move_base/global_costmap/publish_frequency: 0.5
 * /move_base/global_costmap/resolution: 0.2
 * /move_base/global_costmap/robot_base_frame: base_footprint
 * /move_base/global_costmap/robot_radius: 0.2
 * /move_base/global_costmap/rolling_window: True
 * /move_base/global_costmap/static_layer/enabled: True
 * /move_base/global_costmap/static_map: True
 * /move_base/global_costmap/transform_tolerance: 0.5
 * /move_base/global_costmap/update_frequency: 1.0
 * /move_base/global_costmap/width: 100.0
 * /move_base/local_costmap/always_send_full_costmap: True
 * /move_base/local_costmap/global_frame: odom
 * /move_base/local_costmap/height: 4.0
 * /move_base/local_costmap/inflation_layer/cost_scaling_factor: 5.0
 * /move_base/local_costmap/inflation_layer/enabled: True
 * /move_base/local_costmap/inflation_layer/inflation_radius: 0.5
 * /move_base/local_costmap/map_type: costmap
 * /move_base/local_costmap/max_obstacle_height: 0.6
 * /move_base/local_costmap/obstacle_layer/bump/clearing: False
 * /move_base/local_costmap/obstacle_layer/bump/data_type: PointCloud2
 * /move_base/local_costmap/obstacle_layer/bump/marking: True
 * /move_base/local_costmap/obstacle_layer/bump/max_obstacle_height: 0.15
 * /move_base/local_costmap/obstacle_layer/bump/min_obstacle_height: 0.0
 * /move_base/local_costmap/obstacle_layer/bump/topic: mobile_base/senso...
 * /move_base/local_costmap/obstacle_layer/combination_method: 1
 * /move_base/local_costmap/obstacle_layer/enabled: True
 * /move_base/local_costmap/obstacle_layer/mark_threshold: 0
 * /move_base/local_costmap/obstacle_layer/max_obstacle_height: 0.6
 * /move_base/local_costmap/obstacle_layer/observation_sources: scan bump
 * /move_base/local_costmap/obstacle_layer/obstacle_range: 2.5
 * /move_base/local_costmap/obstacle_layer/origin_z: 0.0
 * /move_base/local_costmap/obstacle_layer/publish_voxel_map: False
 * /move_base/local_costmap/obstacle_layer/raytrace_range: 3.0
 * /move_base/local_costmap/obstacle_layer/scan/clearing: True
 * /move_base/local_costmap/obstacle_layer/scan/data_type: LaserScan
 * /move_base/local_costmap/obstacle_layer/scan/marking: True
 * /move_base/local_costmap/obstacle_layer/scan/max_obstacle_height: 0.35
 * /move_base/local_costmap/obstacle_layer/scan/min_obstacle_height: 0.25
 * /move_base/local_costmap/obstacle_layer/scan/topic: scan
 * /move_base/local_costmap/obstacle_layer/track_unknown_space: True
 * /move_base/local_costmap/obstacle_layer/unknown_threshold: 15
 * /move_base/local_costmap/obstacle_layer/z_resolution: 0.2
 * /move_base/local_costmap/obstacle_layer/z_voxels: 2
 * /move_base/local_costmap/plugins: [{'type': 'costma...
 * /move_base/local_costmap/publish_frequency: 2.0
 * /move_base/local_costmap/resolution: 0.01
 * /move_base/local_costmap/robot_base_frame: base_footprint
 * /move_base/local_costmap/robot_radius: 0.2
 * /move_base/local_costmap/rolling_window: True
 * /move_base/local_costmap/static_layer/enabled: True
 * /move_base/local_costmap/static_map: False
 * /move_base/local_costmap/transform_tolerance: 0.5
 * /move_base/local_costmap/update_frequency: 5.0
 * /move_base/local_costmap/width: 4.0
 * /move_base/oscillation_distance: 0.2
 * /move_base/oscillation_timeout: 10.0
 * /move_base/planner_frequency: 1.0
 * /move_base/planner_patience: 1.0
 * /move_base/recovery_behavior_enabled: False
 * /move_base/shutdown_costmaps: False
 * /move_group/allow_trajectory_execution: True
 * /move_group/arm/default_planner_config: RRTConnectkConfig...
 * /move_group/arm/longest_valid_segment_fraction: 0.005
 * /move_group/arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm/projection_evaluator: joints(joint_1,jo...
 * /move_group/capabilities: 
 * /move_group/controller_list: [{'default': True...
 * /move_group/disable_capabilities: 
 * /move_group/gripper/default_planner_config: ESTkConfigDefault
 * /move_group/gripper/longest_valid_segment_fraction: 0.005
 * /move_group/gripper/planner_configs: ['SBLkConfigDefau...
 * /move_group/gripper/projection_evaluator: joints(joint_6,jo...
 * /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_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/octomap_resolution: 0.025
 * /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/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.0
 * /navigation_velocity_smoother/accel_lim_v: 1.0
 * /navigation_velocity_smoother/accel_lim_w: 2.0
 * /navigation_velocity_smoother/decel_factor: 1.5
 * /navigation_velocity_smoother/frequency: 20.0
 * /navigation_velocity_smoother/robot_feedback: 2
 * /navigation_velocity_smoother/speed_lim_v: 0.8
 * /navigation_velocity_smoother/speed_lim_w: 5.4
 * /robot/name: turtlebot
 * /robot/type: turtlebot
 * /robot_description: <?xml version="1....
 * /robot_description_kinematics/arm/kinematics_solver: trac_ik_kinematic...
 * /robot_description_kinematics/arm/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm/kinematics_solver_timeout: 0.005
 * /robot_description_planning/joint_limits/joint_1/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/joint_1/has_velocity_limits: True
 * /robot_description_planning/joint_limits/joint_1/max_acceleration: 0
 * /robot_description_planning/joint_limits/joint_1/max_velocity: 1
 * /robot_description_planning/joint_limits/joint_2/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/joint_2/has_velocity_limits: True
 * /robot_description_planning/joint_limits/joint_2/max_acceleration: 0
 * /robot_description_planning/joint_limits/joint_2/max_velocity: 1
 * /robot_description_planning/joint_limits/joint_3/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/joint_3/has_velocity_limits: True
 * /robot_description_planning/joint_limits/joint_3/max_acceleration: 0
 * /robot_description_planning/joint_limits/joint_3/max_velocity: 1
 * /robot_description_planning/joint_limits/joint_4/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/joint_4/has_velocity_limits: True
 * /robot_description_planning/joint_limits/joint_4/max_acceleration: 0
 * /robot_description_planning/joint_limits/joint_4/max_velocity: 1
 * /robot_description_planning/joint_limits/joint_5/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/joint_5/has_velocity_limits: True
 * /robot_description_planning/joint_limits/joint_5/max_acceleration: 0
 * /robot_description_planning/joint_limits/joint_5/max_velocity: 1
 * /robot_description_planning/joint_limits/joint_6/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/joint_6/has_velocity_limits: True
 * /robot_description_planning/joint_limits/joint_6/max_acceleration: 0
 * /robot_description_planning/joint_limits/joint_6/max_velocity: 1
 * /robot_description_planning/joint_limits/joint_7/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/joint_7/has_velocity_limits: True
 * /robot_description_planning/joint_limits/joint_7/max_acceleration: 0
 * /robot_description_planning/joint_limits/joint_7/max_velocity: 1
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: kinetic
 * /rosversion: 1.12.17
 * /teleop: False
 * /torque_control: False
 * /use_arm: True
 * /use_base: True
 * /use_camera: True
 * /use_sim: False
 * /use_sim_time: False
 * /use_vslam: False

NODES
  /camera/
    color_rectify_color (nodelet/nodelet)
    points_xyzrgb_hw_registered (nodelet/nodelet)
    realsense2_camera (nodelet/nodelet)
    realsense2_camera_manager (nodelet/nodelet)
  /
    bumper2pointcloud (nodelet/nodelet)
    calibration_tf_broadcaster (locobot_calibration/calibration_publish_transforms.py)
    cmd_vel_mux (nodelet/nodelet)
    diagnostic_aggregator (diagnostic_aggregator/aggregator_node)
    locobot_controller (locobot_control/locobot_controller)
    map_server (map_server/map_server)
    mobile_base (nodelet/nodelet)
    mobile_base_nodelet_manager (nodelet/nodelet)
    move_base (move_base/move_base)
    move_group (moveit_ros_move_group/move_group)
    navigation_velocity_smoother (nodelet/nodelet)
    odom_map_broadcaster (tf/static_transform_publisher)
    pyrobot_kinematics (pyrobot_bridge/kinematics.py)
    pyrobot_moveit (pyrobot_bridge/moveit_bridge.py)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    turtlebot_controller_node (turtlebot_controller/turtlebot_controller_node)

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

setting /run_id to 3c4860e8-adc9-11eb-900b-48f17fded622
process[rosout-1]: started with pid [19761]
started core service [/rosout]
process[camera/realsense2_camera_manager-2]: started with pid [19778]
process[camera/realsense2_camera-3]: started with pid [19779]
process[camera/color_rectify_color-4]: started with pid [19780]
process[camera/points_xyzrgb_hw_registered-5]: started with pid [19789]
process[diagnostic_aggregator-6]: started with pid [19802]
process[mobile_base_nodelet_manager-7]: started with pid [19816]
process[mobile_base-8]: started with pid [19836]
process[bumper2pointcloud-9]: started with pid [19841]
process[cmd_vel_mux-10]: started with pid [19856]
process[locobot_controller-11]: started with pid [19861]
process[calibration_tf_broadcaster-12]: started with pid [19879]
process[turtlebot_controller_node-13]: started with pid [19883]
process[pyrobot_kinematics-14]: started with pid [19898]
process[pyrobot_moveit-15]: started with pid [19904]
process[map_server-16]: started with pid [19909]
process[odom_map_broadcaster-17]: started with pid [19927]
process[navigation_velocity_smoother-18]: started with pid [19937]
[ INFO] [1620236539.138876453]: Initializing nodelet with 4 worker threads.
process[move_base-19]: started with pid [19966]
process[robot_state_publisher-20]: started with pid [19970]
process[move_group-21]: started with pid [19980]
[ INFO] [1620236539.378102797]: RealSense ROS v2.2.13
[ INFO] [1620236539.378246349]: Running with LibRealSense v2.33.1
[ INFO] [1620236539.420945859]: Loading group arm from yaml file
[ INFO] [1620236539.421162589]: Loading group camera from yaml file
[ INFO] [1620236539.421217593]: Loading group gripper from yaml file
[ INFO] [1620236539.467326639]: Name : gripperMotor, ID : 7, Model Number : 1060
[ INFO] [1620236539.510255457]:  
[ INFO] [1620236539.514432362]: Device with serial number 913522070688 was found.

[ INFO] [1620236539.514678107]: Device with physical ID /sys/devices/pci0000:00/0000:00:14.0/usb2/2-1/2-1:1.0/video4linux/video0 was found.
[ INFO] [1620236539.514785001]: Device with name Intel RealSense D435 was found.
[ INFO] [1620236539.515314525]: Name : head_pan_joint, ID : 8, Model Number : 1060
[ INFO] [1620236539.521744531]: Device with port number 2-1 was found.
[ INFO] [1620236539.563314308]: Name : head_tilt_joint, ID : 9, Model Number : 1060
[ INFO] [1620236539.588266538]: Loading robot model 'locobot'...
[ INFO] [1620236539.588350089]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1620236539.611349376]: getParameters...
[ INFO] [1620236539.611699505]: Name : joint2Dual, ID : 3, Model Number : 1020
[ INFO] [1620236539.659297247]: Name : joint_1, ID : 1, Model Number : 1020
[ INFO] [1620236539.712840451]: Name : joint_2, ID : 2, Model Number : 1020
[ INFO] [1620236539.755275832]: Name : joint_3, ID : 4, Model Number : 1020
[ INFO] [1620236539.803316921]: Name : joint_4, ID : 5, Model Number : 1020
[ INFO] [1620236539.851295611]: Name : joint_5, ID : 6, Model Number : 1060
[ INFO] [1620236540.334696505]: Using plugin "static_layer"
[ INFO] [1620236540.360584463]: Requesting the map...
[ INFO] [1620236540.367899845]: setupDevice...
[ INFO] [1620236540.368023399]: JSON file is not provided
[ INFO] [1620236540.368093834]: ROS Node Namespace: camera
[ INFO] [1620236540.368162586]: Device Name: Intel RealSense D435
[ INFO] [1620236540.368229870]: Device Serial No: 913522070688
[ INFO] [1620236540.368297937]: Device physical port: /sys/devices/pci0000:00/0000:00:14.0/usb2/2-1/2-1:1.0/video4linux/video0
[ INFO] [1620236540.368366863]: Device FW version: 05.11.04.00
[ INFO] [1620236540.368434701]: Device Product ID: 0x0B07
[ INFO] [1620236540.368502789]: Enable PointCloud: Off
[ INFO] [1620236540.368569528]: Align Depth: On
[ INFO] [1620236540.368636585]: Sync Mode: On
[ INFO] [1620236540.368744061]: Device Sensors: 
[ INFO] [1620236540.368842056]: Stereo Module was found.
[ INFO] [1620236540.368920855]: RGB Camera was found.
[ INFO] [1620236540.369003365]: (Fisheye, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1620236540.369071888]: (Fisheye, 1) sensor isn't supported by current device! -- Skipping...
[ INFO] [1620236540.369143479]: (Fisheye, 2) sensor isn't supported by current device! -- Skipping...
[ INFO] [1620236540.369210996]: (Gyro, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1620236540.369277959]: (Accel, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1620236540.369338119]: (Pose, 0) sensor isn't supported by current device! -- Skipping...
[ INFO] [1620236540.369407446]: num_filters: 0
[ INFO] [1620236540.369464235]: Setting Dynamic reconfig parameters.
[ INFO] [1620236540.423597329]: Done Setting Dynamic reconfig parameters.
[ INFO] [1620236540.428325763]: depth stream is enabled - width: 640, height: 480, fps: 30, Format: Z16
[ INFO] [1620236540.428612426]: infra1 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[ INFO] [1620236540.428835143]: infra2 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[ INFO] [1620236540.434815974]: color stream is enabled - width: 640, height: 480, fps: 30, Format: RGB8
[ INFO] [1620236540.437065925]: setupPublishers...
[ INFO] [1620236540.440977334]: Expected frequency for depth = 30.00000
[WARN] [1620236540.463556]: Will publish: 
[WARN] [1620236540.463732]:   head_tilt_link to camera_link
[ INFO] [1620236540.479378093]: Expected frequency for infra1 = 30.00000
[ INFO] [1620236540.510519364]: Expected frequency for aligned_depth_to_infra1 = 30.00000
[ INFO] [1620236540.567850822]: Resizing static layer to 600 X 600 at 0.010000 m/pix
[ INFO] [1620236540.590789888]: Expected frequency for infra2 = 30.00000
[ INFO] [1620236540.656069661]: Expected frequency for color = 30.00000
[ INFO] [1620236540.664951095]: IK Using joint shoulder_link -1.57 1.57
[ INFO] [1620236540.664979216]: IK Using joint elbow_link -1.57 1.57
[ INFO] [1620236540.664991000]: IK Using joint forearm_link -1.57 1.57
[ INFO] [1620236540.665007535]: IK Using joint wrist_link -1.57 1.57
[ INFO] [1620236540.665021404]: IK Using joint gripper_link -1.57 1.57
[ INFO] [1620236540.665043469]: Looking in common namespaces for param name: arm/position_only_ik
[ INFO] [1620236540.667669463]: Received a 600 X 600 map at 0.010000 m/pix
[ INFO] [1620236540.670475682]: Looking in common namespaces for param name: arm/solve_type
[ INFO] [1620236540.677156381]: Using solve type Speed
[ INFO] [1620236540.679137119]: Using plugin "obstacle_layer"
[ INFO] [1620236540.687488936]:     Subscribed to Topics: scan bump
[ INFO] [1620236540.727225258]: Expected frequency for aligned_depth_to_color = 30.00000
[ INFO] [1620236540.795231179]: setupStreams...
[ INFO] [1620236540.796077951]: insert Depth to Stereo Module
[ INFO] [1620236540.796158235]: insert Color to RGB Camera
[ INFO] [1620236540.796185046]: insert Infrared to Stereo Module
[ INFO] [1620236540.796216726]: insert Infrared to Stereo Module
[ INFO] [1620236540.797134061]: Using plugin "inflation_layer"
[ INFO] [1620236540.808623599]: SELECTED BASE:Depth, 0
[ INFO] [1620236540.833235812]: RealSense Node Is Up!
[ INFO] [1620236540.873606994]: Using plugin "static_layer"
[ INFO] [1620236540.877134988]: Requesting the map...
[ INFO] [1620236540.879347914]: Resizing static layer to 600 X 600 at 0.010000 m/pix
[ INFO] [1620236540.971209695]: [DynamixelDriver] Succeeded to add sync write handler
[ INFO] [1620236540.971241894]: [DynamixelDriver] Succeeded to add sync write handler
[ INFO] [1620236540.979183679]: Received a 600 X 600 map at 0.010000 m/pix
[ INFO] [1620236540.985722083]: Using plugin "obstacle_layer"
[ INFO] [1620236540.990443150]:     Subscribed to Topics: scan bump
[ INFO] [1620236541.054900784]: Using plugin "inflation_layer"
[ INFO] [1620236541.097825229]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1620236541.107681136]: Sim period is set to 0.20
[ INFO] [1620236541.903200306]: Recovery behavior will clear layer obstacles
[ INFO] [1620236541.908098363]: Recovery behavior will clear layer obstacles
[ INFO] [1620236541.939762040]: odom received!
[ INFO] [1620236542.332173872]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1620236542.334777956]: MoveGroup debug mode is ON
Starting context monitors...
[ INFO] [1620236542.334813572]: Starting scene monitor
[ INFO] [1620236542.337240190]: Listening to '/planning_scene'
[ INFO] [1620236542.337282533]: Starting world geometry monitor
[ INFO] [1620236542.339829613]: Listening to '/collision_object' using message notifier with target frame '/base_footprint '
[ INFO] [1620236542.342241466]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1620236552.518077654]: Listening to '/head_mount_kinect/depth_registered/points' using message filter with target frame '/base_footprint '
[ INFO] [1620236552.523257239]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1620236552.547059891]: Initializing OMPL interface using ROS parameters
[ INFO] [1620236552.584531814]: Using planning interface 'OMPL'
[ INFO] [1620236552.587417598]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1620236552.587983057]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1620236552.588412112]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1620236552.588869789]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1620236552.589321435]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1620236552.589773863]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1620236552.589838060]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1620236552.589861034]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1620236552.589879195]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1620236552.589896364]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1620236552.589914017]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1620236552.873757713]: Added FollowJointTrajectory controller for locobot_arm/joint_controller
[ INFO] [1620236552.873939915]: Returned 1 controllers in list
[ INFO] [1620236552.907441520]: 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] [1620236552.981960066]: 

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

[ INFO] [1620236552.982034774]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1620236552.982061540]: MoveGroup context initialization complete

You can start planning now!

I can try reinstalling the whole robot, maybe that will help. Should I use master or Develop branch on the new installation?

Jekyll1021 commented 3 years ago

I tested on master branch. Could you check if get_rgb_depth() is returning value properly?

Jendker commented 3 years ago

I'm on master branch as well now. The first component returns has values, only depth is all zeros. Calling rostopic echo /camera/depth_registered/points still gives me values != 0 as expected.

Jekyll1021 commented 3 years ago

Could you try rostopic echo /camera/aligned_depth_to_color/image_raw? This is the depth topic locobot subscribes to.

Jendker commented 3 years ago

Still looks good!

Jekyll1021 commented 3 years ago

Could you poke into this line https://github.com/facebookresearch/pyrobot/blob/13a70d2427417756504ca4ff3cf99a33f15abbde/src/pyrobot/core.py#L364 to see if self.depth_img update makes sense? Reinstalling would most likely help, but I hope we can pinpoint the problem before doing so - if something pops up after reinstall, it would be exponentially harder to debug.

Jendker commented 3 years ago

self.depth_img looks fine, I am getting non-zero outputs

Jekyll1021 commented 3 years ago

Okay sounds good. At this point, I actually suggest against reinstalling, since the problem should be somewhere between subscribe image and returning depth.

Could you check the depth here https://github.com/facebookresearch/pyrobot/blob/13a70d2427417756504ca4ff3cf99a33f15abbde/src/pyrobot/core.py#L411

by calling get_rgb_depth?

Jendker commented 3 years ago

Hmm looks like python2 and 3 compatibility issue. I am still running python2 and the function is running int division by self.configs.CAMERA.DEPTH_MAP_FACTOR. I have fixed that by adding float(...) here https://github.com/facebookresearch/pyrobot/blob/13a70d2427417756504ca4ff3cf99a33f15abbde/src/pyrobot/core.py#L397 and here https://github.com/facebookresearch/pyrobot/blob/13a70d2427417756504ca4ff3cf99a33f15abbde/src/pyrobot/core.py#L412 but I am still getting empty array. What other place do I need to adjust?

Jekyll1021 commented 3 years ago

I want to see if the raw depth before division is giving values (and also in meter). If the raw depth is giving actual value but zeroed out after a division, realsense might be giving meter reading instead of millimeter reading, or there's some other problem, like the formatting problem you just caught.

https://github.com/facebookresearch/pyrobot/blob/13a70d2427417756504ca4ff3cf99a33f15abbde/src/pyrobot/core.py#L411

For me, running a fresh install gives millimeter reading. But tbh there's quite a number of upstreams to manager here, so I won't be surprised if some format / conversion changed.

Jendker commented 3 years ago

I see, so I may need to adjust the units myself. Here are the values of depth I am getting in the line mentioned by you:

[[864 875 875 ... 921 921 921]
 [861 871 871 ... 921 921 921]
 [861 871 871 ... 917 917 918]
 ...
 [526 526 526 ... 521 521 521]
 [526 526 526 ... 521 521 521]
 [526 526 525 ... 520 520 520]]

Jekyll1021 commented 3 years ago

I see, so I may need to adjust the units myself. Here are the values of depth I am getting in the line mentioned by you:
[[864 875 875 ... 921 921 921]
 [861 871 871 ... 921 921 921]
 [861 871 871 ... 917 917 918]
 ...
 [526 526 526 ... 521 521 521]
 [526 526 526 ... 521 521 521]
 [526 526 525 ... 520 520 520]]

These values look like proper millimeter readings to me. Does depth give all zeros at https://github.com/facebookresearch/pyrobot/blob/13a70d2427417756504ca4ff3cf99a33f15abbde/src/pyrobot/core.py#L412?

If so, could you check what's the type and value of depth and self.configs.CAMERA.DEPTH_MAP_FACTOR?

Jendker commented 3 years ago

depth is all zeros after division in line 412 and is a np.ndarray with dtype uint16. self.configs.CAMERA.DEPTH_MAP_FACTOR is of int type.

Jekyll1021 commented 3 years ago

This is indeed the problem. Could you change the line to depth = depth.astype(np.float32) / 1000 ? Seems like a py2 bumpy typing problem as you mentioned.

Jendker commented 3 years ago

Hmm after the adjustment depth value is:

[[0.864 0.875 0.875 ... 0.921 0.921 0.921]
 [0.861 0.871 0.871 ... 0.921 0.921 0.921]
 [0.861 0.871 0.871 ... 0.917 0.917 0.918]
 ...
 [0.526 0.526 0.526 ... 0.521 0.521 0.521]
 [0.526 0.526 0.526 ... 0.521 0.521 0.521]
 [0.525 0.525 0.525 ... 0.52  0.52  0.52 ]]

but the list returned by get_current_pcd is still empty. Do you recall if there is any other division I should adjust?

Jekyll1021 commented 3 years ago

Could you check depth before and after this line?

https://github.com/facebookresearch/pyrobot/blob/13a70d2427417756504ca4ff3cf99a33f15abbde/src/pyrobot/locobot/camera.py#L441

Jendker commented 3 years ago

No problem!

Before:

[[0.864 0.874 0.874 ... 0.921 0.921 0.921]
 [0.861 0.871 0.871 ... 0.921 0.921 0.921]
 [0.861 0.871 0.871 ... 0.918 0.918 0.918]
 ...
 [0.527 0.527 0.527 ... 0.522 0.521 0.521]
 [0.527 0.527 0.527 ... 0.522 0.521 0.521]
 [0.526 0.526 0.526 ... 0.521 0.52  0.52 ]]

after:

[0.000864 0.000874 0.000874 ... 0.000521 0.00052  0.00052 ]

Jekyll1021 commented 3 years ago

That's why. Change the line to depth = depth_im.reshape(-1) should solve the problem!

Jendker commented 3 years ago

We are back in business! The second returned value of get_current_pcd is of np.ndarray and has dtype uint8 in contrast to depth which is float32. Is that fine?

Jekyll1021 commented 3 years ago

The second return value is RGB of point cloud which is suppose to be uint8.

Jendker commented 3 years ago

Thank you for the help!

facebookresearch / pyrobot