fetch_gazebo icon indicating copy to clipboard operation
fetch_gazebo copied to clipboard

Published 20 hours ago • verified publisher icon ZebraDevs

[demo.py] the robot keeps rotating after moved near table

Open umhan35 opened this issue 5 years ago • 4 comments

Hi, I am using Ubuntu 18.04 and ROS Melodic (to align with the version that the FetchIt robot has).

I followed the gazabo tutorial (https://docs.fetchrobotics.com/gazebo.html) and run the following:

roslaunch fetch_gazebo playground.launch
roslaunch fetch_gazebo_demo demo.launch

I expect the robot will move to the table and pick the blue cube, but after the robot moved near table keeps rotating it self. During 2 times, I waited 10 minutes before "Aborting because a valid control could not be found. Even after executing all recovery behaviors"

Rotating: r

r2

r3

Here is the excerpt of the log:

[INFO] [1550152825.917689, 39.862000]: Waiting for basic_grasping_perception/find_objects...
[INFO] [1550152826.167008, 39.894000]: Moving to table...
[ WARN] [1550152935.337158025, 67.535000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550152980.132707802, 77.615000000]: Rotate recovery behavior started.
[ WARN] [1550153058.423329389, 95.251000000]: Invalid Trajectory 0.093694, 0.000000, 0.025957, cost: -1.000000
[ WARN] [1550153093.262487593, 103.608000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153134.243491509, 113.969000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153178.759782706, 125.168000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153254.751576573, 143.775000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153327.259470285, 162.218000000]: Rotate recovery behavior started.
[ WARN] [1550153366.278706368, 172.399000000]: Clearing costmap to unstuck robot (0.500000m).
[ERROR] [1550153366.753323531, 172.518000000]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
[INFO] [1550153366.785093, 172.526000]: Raising torso...
Get block to pick...
[INFO] [1550153391.572698, 178.801000]: Picking object...
[WARN] [1550153392.673143, 178.988000]: ObjectManager: object0 not added yet
[WARN] [1550153392.681189, 178.992000]: ObjectManager: object1 not added yet
[WARN] [1550153392.688479, 178.993000]: ObjectManager: object2 not added yet
[WARN] [1550153392.695956, 178.993000]: ObjectManager: surface0 not added yet
[WARN] [1550153392.702579, 178.996000]: ObjectManager: surface1 not added yet
[INFO] [1550153393.662212, 179.207000]: Beginning to pick.
[ INFO] [1550153393.710886642, 179.216000000]: Planning attempt 1 of at most 1

Here is the full log:

$ roslaunch fetch_gazebo_demo demo.launch
... logging to /home/zhao/.ros/log/968350b0-3060-11e9-8d6a-04d3b0082825/roslaunch-zhao1804-15024.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://zhao1804:45795/

SUMMARY
========

PARAMETERS
 * /amcl/base_frame_id: base_link
 * /amcl/global_frame_id: map
 * /amcl/gui_publish_rate: 10.0
 * /amcl/kld_err: 0.01
 * /amcl/kld_z: 0.99
 * /amcl/laser_likelihood_max_dist: 2.0
 * /amcl/laser_max_beams: 30
 * /amcl/laser_max_range: -1.0
 * /amcl/laser_min_range: -1.0
 * /amcl/laser_model_type: likelihood_field
 * /amcl/laser_sigma_hit: 0.2
 * /amcl/laser_z_hit: 0.95
 * /amcl/laser_z_rand: 0.05
 * /amcl/max_particles: 2000
 * /amcl/min_particles: 500
 * /amcl/odom_alpha1: 0.15
 * /amcl/odom_alpha2: 0.5
 * /amcl/odom_alpha3: 0.5
 * /amcl/odom_alpha4: 0.15
 * /amcl/odom_frame_id: odom
 * /amcl/odom_model_type: diff-corrected
 * /amcl/recovery_alpha_fast: 0.0
 * /amcl/recovery_alpha_slow: 0.0
 * /amcl/resample_interval: 2
 * /amcl/save_pose_rate: 0.5
 * /amcl/transform_tolerance: 0.5
 * /amcl/update_min_a: 0.25
 * /amcl/update_min_d: 0.1
 * /amcl/use_map_topic: False
 * /basic_grasping_perception/gripper/approach/desired: 0.15
 * /basic_grasping_perception/gripper/approach/min: 0.145
 * /basic_grasping_perception/gripper/finger_depth: 0.02
 * /basic_grasping_perception/gripper/gripper_tolerance: 0.05
 * /basic_grasping_perception/gripper/retreat/desired: 0.15
 * /basic_grasping_perception/gripper/retreat/min: 0.145
 * /basic_grasping_perception/gripper/tool_to_planning_frame: 0.18
 * /move_base/NavfnROS/allow_unknown: True
 * /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: 0.8
 * /move_base/TrajectoryPlannerROS/acc_lim_x: 0.35
 * /move_base/TrajectoryPlannerROS/acc_lim_y: 0.0
 * /move_base/TrajectoryPlannerROS/angular_sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/dwa: False
 * /move_base/TrajectoryPlannerROS/gdist_scale: 12.0
 * /move_base/TrajectoryPlannerROS/heading_lookahead: 0.325
 * /move_base/TrajectoryPlannerROS/heading_scoring: True
 * /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: 1.5
 * /move_base/TrajectoryPlannerROS/max_vel_x: 1.0
 * /move_base/TrajectoryPlannerROS/max_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/meter_scoring: True
 * /move_base/TrajectoryPlannerROS/min_in_place_vel_theta: 0.3
 * /move_base/TrajectoryPlannerROS/min_vel_theta: -1.5
 * /move_base/TrajectoryPlannerROS/min_vel_x: 0.15
 * /move_base/TrajectoryPlannerROS/min_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/occdist_scale: 0.1
 * /move_base/TrajectoryPlannerROS/oscillation_reset_dist: 0.05
 * /move_base/TrajectoryPlannerROS/pdist_scale: 8.0
 * /move_base/TrajectoryPlannerROS/publish_cost_grid_pc: False
 * /move_base/TrajectoryPlannerROS/sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/sim_time: 1.0
 * /move_base/TrajectoryPlannerROS/vtheta_samples: 10
 * /move_base/TrajectoryPlannerROS/vx_samples: 3
 * /move_base/TrajectoryPlannerROS/xy_goal_tolerance: 0.1
 * /move_base/TrajectoryPlannerROS/yaw_goal_tolerance: 0.1
 * /move_base/aggressive_reset/reset_distance: 0.5
 * /move_base/base_global_planner: navfn/NavfnROS
 * /move_base/base_local_planner: base_local_planne...
 * /move_base/conservative_reset/reset_distance: 3.0
 * /move_base/controller_frequency: 25.0
 * /move_base/controller_patience: 15.0
 * /move_base/global_costmap/global_frame: map
 * /move_base/global_costmap/inflater/inflation_radius: 0.7
 * /move_base/global_costmap/inflater/robot_radius: 0.3
 * /move_base/global_costmap/obstacles/base_scan/clearing: True
 * /move_base/global_costmap/obstacles/base_scan/data_type: LaserScan
 * /move_base/global_costmap/obstacles/base_scan/marking: True
 * /move_base/global_costmap/obstacles/base_scan/max_obstacle_height: 0.3
 * /move_base/global_costmap/obstacles/base_scan/min_obstacle_height: 0.25
 * /move_base/global_costmap/obstacles/base_scan/obstacle_range: 4.0
 * /move_base/global_costmap/obstacles/base_scan/raytrace_range: 5.0
 * /move_base/global_costmap/obstacles/base_scan/topic: base_scan
 * /move_base/global_costmap/obstacles/max_obstacle_height: 2.0
 * /move_base/global_costmap/obstacles/observation_sources: base_scan
 * /move_base/global_costmap/obstacles/z_resolution: 0.125
 * /move_base/global_costmap/obstacles/z_voxels: 16
 * /move_base/global_costmap/plugins: [{'type': 'costma...
 * /move_base/global_costmap/publish_frequency: 0.0
 * /move_base/global_costmap/robot_base_frame: base_link
 * /move_base/global_costmap/robot_radius: 0.3
 * /move_base/global_costmap/static_map: True
 * /move_base/global_costmap/transform_tolerance: 0.5
 * /move_base/global_costmap/update_frequency: 5.0
 * /move_base/local_costmap/global_frame: odom
 * /move_base/local_costmap/height: 4.0
 * /move_base/local_costmap/inflater/inflation_radius: 0.7
 * /move_base/local_costmap/inflater/robot_radius: 0.3
 * /move_base/local_costmap/obstacles/base_scan/clearing: True
 * /move_base/local_costmap/obstacles/base_scan/data_type: LaserScan
 * /move_base/local_costmap/obstacles/base_scan/marking: True
 * /move_base/local_costmap/obstacles/base_scan/max_obstacle_height: 0.3
 * /move_base/local_costmap/obstacles/base_scan/min_obstacle_height: 0.25
 * /move_base/local_costmap/obstacles/base_scan/obstacle_range: 4.0
 * /move_base/local_costmap/obstacles/base_scan/raytrace_range: 5.0
 * /move_base/local_costmap/obstacles/base_scan/topic: base_scan
 * /move_base/local_costmap/obstacles/max_obstacle_height: 2.0
 * /move_base/local_costmap/obstacles/observation_sources: base_scan
 * /move_base/local_costmap/obstacles/publish_observations: False
 * /move_base/local_costmap/obstacles/z_resolution: 0.125
 * /move_base/local_costmap/obstacles/z_voxels: 16
 * /move_base/local_costmap/plugins: [{'type': 'costma...
 * /move_base/local_costmap/publish_frequency: 2.0
 * /move_base/local_costmap/resolution: 0.025
 * /move_base/local_costmap/robot_base_frame: base_link
 * /move_base/local_costmap/robot_radius: 0.3
 * /move_base/local_costmap/rolling_window: True
 * /move_base/local_costmap/transform_tolerance: 0.5
 * /move_base/local_costmap/width: 4.0
 * /move_base/oscillation_distance: 0.5
 * /move_base/oscillation_timeout: 10.0
 * /move_base/planner_frequency: 0.0
 * /move_base/planner_patience: 5.0
 * /move_base/recovery_behavior_enabled: True
 * /move_base/recovery_behaviors: [{'type': 'clear_...
 * /move_base/rotate_recovery/frequency: 20.0
 * /move_base/rotate_recovery/sim_granularity: 0.017
 * /move_group/allow_trajectory_execution: True
 * /move_group/allowed_execution_duration_scaling: 1.2
 * /move_group/allowed_goal_duration_margin: 0.5
 * /move_group/arm/longest_valid_segment_fraction: 0.05
 * /move_group/arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm/projection_evaluator: joints(shoulder_p...
 * /move_group/arm_with_torso/longest_valid_segment_fraction: 0.05
 * /move_group/arm_with_torso/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm_with_torso/projection_evaluator: joints(torso_lift...
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'default': True...
 * /move_group/gripper/planner_configs: ['SBLkConfigDefau...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /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/start_state_max_bounds_error: 0.1
 * /robot_description_kinematics/arm/kinematics_solver: fetch_arm_kinemat...
 * /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_kinematics/arm_with_torso/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_timeout: 0.005
 * /robot_description_planning/joint_limits/elbow_flex_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/elbow_flex_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/elbow_flex_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/elbow_flex_joint/max_velocity: 1.5
 * /robot_description_planning/joint_limits/forearm_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/forearm_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/forearm_roll_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/forearm_roll_joint/max_velocity: 1.57
 * /robot_description_planning/joint_limits/left_gripper_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/left_gripper_joint/has_velocity_limits: False
 * /robot_description_planning/joint_limits/left_gripper_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/left_gripper_joint/max_velocity: 0
 * /robot_description_planning/joint_limits/right_gripper_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/right_gripper_joint/has_velocity_limits: False
 * /robot_description_planning/joint_limits/right_gripper_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/right_gripper_joint/max_velocity: 0
 * /robot_description_planning/joint_limits/shoulder_lift_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/shoulder_lift_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_acceleration: 1.0
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity: 1.0
 * /robot_description_planning/joint_limits/shoulder_pan_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/shoulder_pan_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity: 1.256
 * /robot_description_planning/joint_limits/torso_lift_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/torso_lift_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/torso_lift_joint/max_acceleration: 0.5
 * /robot_description_planning/joint_limits/torso_lift_joint/max_velocity: 0.1
 * /robot_description_planning/joint_limits/upperarm_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/upperarm_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/upperarm_roll_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/upperarm_roll_joint/max_velocity: 1.57
 * /robot_description_planning/joint_limits/wrist_flex_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_flex_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_flex_joint/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/wrist_flex_joint/max_velocity: 2.26
 * /robot_description_planning/joint_limits/wrist_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_roll_joint/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/wrist_roll_joint/max_velocity: 2.26
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: melodic
 * /rosversion: 1.14.3

NODES
  /
    amcl (amcl/amcl)
    basic_grasping_perception (simple_grasping/basic_grasping_perception)
    demo (fetch_gazebo_demo/demo.py)
    map_server (map_server/map_server)
    move_base (move_base/move_base)
    move_group (moveit_ros_move_group/move_group)
    tilt_head_node (fetch_navigation/tilt_head.py)

ROS_MASTER_URI=http://localhost:11311

running rosparam delete /move_group/sensors
ERROR: parameter [/move_group/sensors] is not set
process[map_server-1]: started with pid [15068]
process[amcl-2]: started with pid [15069]
process[move_base-3]: started with pid [15074]
process[tilt_head_node-4]: started with pid [15081]
process[move_group-5]: started with pid [15086]
process[basic_grasping_perception-6]: started with pid [15088]
process[demo-7]: started with pid [15089]
[ INFO] [1550152754.618718786]: Loading robot model 'fetch'...
[ INFO] [1550152754.623928359]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1550152755.474892647, 21.301000000]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ WARN] [1550152755.521153797, 21.306000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1550152756.164802904, 21.364000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1550152756.184662846, 21.367000000]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1550152756.184855454, 21.367000000]: Starting planning scene monitor
[ INFO] [1550152756.196402249, 21.368000000]: Listening to '/planning_scene'
[ INFO] [1550152756.196794411, 21.368000000]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1550152756.218372224, 21.371000000]: Listening to '/collision_object' using message notifier with target frame 'base_link '
[ INFO] [1550152756.233318116, 21.373000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1550152756.235112436, 21.374000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1550152756.429797862, 21.400000000]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1550152756.560505984, 21.408000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1550152756.684014541, 21.416000000]: Using planning interface 'OMPL'
[ INFO] [1550152756.705880057, 21.418000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1550152756.710598888, 21.418000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1550152756.720895651, 21.419000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550152756.726043575, 21.419000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550152756.728372573, 21.419000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1550152756.730569858, 21.419000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1550152756.731436529, 21.419000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1550152756.732194740, 21.419000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1550152756.732704201, 21.420000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1550152756.733383308, 21.420000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1550152756.733822725, 21.420000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1550152757.035567134, 21.454000000]: Added FollowJointTrajectory controller for arm_controller
[ INFO] [1550152757.292258046, 21.492000000]: Added FollowJointTrajectory controller for arm_with_torso_controller
[ INFO] [1550152757.548852007, 21.519000000]: Added GripperCommand controller for gripper_controller
[ INFO] [1550152757.549141987, 21.519000000]: Returned 3 controllers in list
[ INFO] [1550152757.616081913, 21.525000000]: 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] [1550152757.942615278, 21.566000000]: 

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

[ INFO] [1550152757.942731993, 21.566000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1550152757.942764210, 21.566000000]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1550152759.487356168, 21.810000000]: Using plugin "static_map"
[ INFO] [1550152759.528604885, 21.815000000]: Requesting the map...
[ INFO] [1550152760.131376396, 21.918000000]: Resizing costmap to 401 X 444 at 0.050000 m/pix
[ INFO] [1550152760.625641725, 22.018000000]: Received a 401 X 444 map at 0.050000 m/pix
[ INFO] [1550152760.636009950, 22.018000000]: Using plugin "obstacles"
[ INFO] [1550152760.951294745, 22.052000000]:     Subscribed to Topics: base_scan
[INFO] [1550152761.129958, 22.093000]: Waiting for move_base...
[ INFO] [1550152761.214008339, 22.106000000]: Using plugin "inflater"
[ INFO] [1550152761.424443860, 22.147000000]: Using plugin "obstacles"
[ INFO] [1550152761.438196843, 22.149000000]:     Subscribed to Topics: base_scan
[ INFO] [1550152761.605159864, 22.173000000]: Using plugin "inflater"
[ INFO] [1550152761.788268598, 22.211000000]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1550152761.829801498, 22.220000000]: Sim period is set to 0.04
[ INFO] [1550152762.578927711, 22.404000000]: Recovery behavior will clear layer obstacles
[ INFO] [1550152762.631120344, 22.417000000]: Recovery behavior will clear layer obstacles
[ INFO] [1550152762.796226307, 22.443000000]: odom received!
[INFO] [1550152762.949895, 22.481000]: Waiting for torso_controller...
[INFO] [1550152763.131925, 22.523000]: Waiting for head_controller...
[INFO] [1550152763.396435, 22.593000]: Waiting for get_planning_scene
[INFO] [1550152763.426339, 22.600000]: Connecting to pickup action...
[INFO] [1550152763.729734, 22.676000]: ...connected
[INFO] [1550152763.732230, 22.677000]: Connecting to place action...
[INFO] [1550152763.958943, 22.731000]: ...connected
[INFO] [1550152764.263868, 22.806000]: Waiting for basic_grasping_perception/find_objects...
Get block to pick...
[INFO] [1550152764.535517, 22.855000]: Picking object...
[WARN] [1550152765.726596, 23.118000]: Perception failed.
[INFO] [1550152765.731775, 23.118000]: Picking object...
[WARN] [1550152766.800970, 23.412000]: Perception failed.
[INFO] [1550152766.805500, 23.412000]: Picking object...
[WARN] [1550152767.817863, 23.676000]: Perception failed.
[INFO] [1550152767.831221, 23.679000]: Picking object...
[WARN] [1550152768.789852, 23.945000]: Perception failed.
[INFO] [1550152768.792502, 23.945000]: Picking object...
[WARN] [1550152769.793836, 24.212000]: Perception failed.
[INFO] [1550152769.796557, 24.212000]: Picking object...
[WARN] [1550152770.793096, 24.480000]: Perception failed.
[INFO] [1550152770.796807, 24.481000]: Picking object...
^C[basic_grasping_perception-6] killing on exit
[demo-7] killing on exit
[move_group-5] killing on exit
[tilt_head_node-4] killing on exit
[move_base-3] killing on exit
[amcl-2] killing on exit
[map_server-1] killing on exit
Traceback (most recent call last):
  File "/home/zhao/Dropbox/fetchit/catkin_ws/src/fetch_gazebo/fetch_gazebo_demo/scripts/demo.py", line 279, in <module>
    grasping_client.updateScene()
  File "/home/zhao/Dropbox/fetchit/catkin_ws/src/fetch_gazebo/fetch_gazebo_demo/scripts/demo.py", line 172, in updateScene
    self.scene.waitForSync()
  File "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_python/planning_scene_interface.py", line 410, in waitForSync
    rospy.sleep(0.1)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
    raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
Traceback (most recent call last):
  File "/home/zhao/Dropbox/fetchit/catkin_ws/src/fetch_ros/fetch_navigation/scripts/tilt_head.py", line 138, in <module>
    h.loop()
  File "/home/zhao/Dropbox/fetchit/catkin_ws/src/fetch_ros/fetch_navigation/scripts/tilt_head.py", line 133, in loop
    rospy.sleep(1.0)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/timer.py", line 165, in sleep
    raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
rospy.exceptions.ROSInterruptException: ROS shutdown request
shutting down processing monitor...
... shutting down processing monitor complete
done
zhao@zhao1804:~/Dropbox/fetchit/catkin_ws$ roslaunch fetch_gazebo_demo demo.launch
... logging to /home/zhao/.ros/log/968350b0-3060-11e9-8d6a-04d3b0082825/roslaunch-zhao1804-15691.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://zhao1804:37927/

SUMMARY
========

PARAMETERS
 * /amcl/base_frame_id: base_link
 * /amcl/global_frame_id: map
 * /amcl/gui_publish_rate: 10.0
 * /amcl/kld_err: 0.01
 * /amcl/kld_z: 0.99
 * /amcl/laser_likelihood_max_dist: 2.0
 * /amcl/laser_max_beams: 30
 * /amcl/laser_max_range: -1.0
 * /amcl/laser_min_range: -1.0
 * /amcl/laser_model_type: likelihood_field
 * /amcl/laser_sigma_hit: 0.2
 * /amcl/laser_z_hit: 0.95
 * /amcl/laser_z_rand: 0.05
 * /amcl/max_particles: 2000
 * /amcl/min_particles: 500
 * /amcl/odom_alpha1: 0.15
 * /amcl/odom_alpha2: 0.5
 * /amcl/odom_alpha3: 0.5
 * /amcl/odom_alpha4: 0.15
 * /amcl/odom_frame_id: odom
 * /amcl/odom_model_type: diff-corrected
 * /amcl/recovery_alpha_fast: 0.0
 * /amcl/recovery_alpha_slow: 0.0
 * /amcl/resample_interval: 2
 * /amcl/save_pose_rate: 0.5
 * /amcl/transform_tolerance: 0.5
 * /amcl/update_min_a: 0.25
 * /amcl/update_min_d: 0.1
 * /amcl/use_map_topic: False
 * /basic_grasping_perception/gripper/approach/desired: 0.15
 * /basic_grasping_perception/gripper/approach/min: 0.145
 * /basic_grasping_perception/gripper/finger_depth: 0.02
 * /basic_grasping_perception/gripper/gripper_tolerance: 0.05
 * /basic_grasping_perception/gripper/retreat/desired: 0.15
 * /basic_grasping_perception/gripper/retreat/min: 0.145
 * /basic_grasping_perception/gripper/tool_to_planning_frame: 0.18
 * /move_base/NavfnROS/allow_unknown: True
 * /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: 0.8
 * /move_base/TrajectoryPlannerROS/acc_lim_x: 0.35
 * /move_base/TrajectoryPlannerROS/acc_lim_y: 0.0
 * /move_base/TrajectoryPlannerROS/angular_sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/dwa: False
 * /move_base/TrajectoryPlannerROS/gdist_scale: 12.0
 * /move_base/TrajectoryPlannerROS/heading_lookahead: 0.325
 * /move_base/TrajectoryPlannerROS/heading_scoring: True
 * /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: 1.5
 * /move_base/TrajectoryPlannerROS/max_vel_x: 1.0
 * /move_base/TrajectoryPlannerROS/max_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/meter_scoring: True
 * /move_base/TrajectoryPlannerROS/min_in_place_vel_theta: 0.3
 * /move_base/TrajectoryPlannerROS/min_vel_theta: -1.5
 * /move_base/TrajectoryPlannerROS/min_vel_x: 0.15
 * /move_base/TrajectoryPlannerROS/min_vel_y: 0.0
 * /move_base/TrajectoryPlannerROS/occdist_scale: 0.1
 * /move_base/TrajectoryPlannerROS/oscillation_reset_dist: 0.05
 * /move_base/TrajectoryPlannerROS/pdist_scale: 8.0
 * /move_base/TrajectoryPlannerROS/publish_cost_grid_pc: False
 * /move_base/TrajectoryPlannerROS/sim_granularity: 0.025
 * /move_base/TrajectoryPlannerROS/sim_time: 1.0
 * /move_base/TrajectoryPlannerROS/vtheta_samples: 10
 * /move_base/TrajectoryPlannerROS/vx_samples: 3
 * /move_base/TrajectoryPlannerROS/xy_goal_tolerance: 0.1
 * /move_base/TrajectoryPlannerROS/yaw_goal_tolerance: 0.1
 * /move_base/aggressive_reset/reset_distance: 0.5
 * /move_base/base_global_planner: navfn/NavfnROS
 * /move_base/base_local_planner: base_local_planne...
 * /move_base/conservative_reset/reset_distance: 3.0
 * /move_base/controller_frequency: 25.0
 * /move_base/controller_patience: 15.0
 * /move_base/global_costmap/global_frame: map
 * /move_base/global_costmap/inflater/inflation_radius: 0.7
 * /move_base/global_costmap/inflater/robot_radius: 0.3
 * /move_base/global_costmap/obstacles/base_scan/clearing: True
 * /move_base/global_costmap/obstacles/base_scan/data_type: LaserScan
 * /move_base/global_costmap/obstacles/base_scan/marking: True
 * /move_base/global_costmap/obstacles/base_scan/max_obstacle_height: 0.3
 * /move_base/global_costmap/obstacles/base_scan/min_obstacle_height: 0.25
 * /move_base/global_costmap/obstacles/base_scan/obstacle_range: 4.0
 * /move_base/global_costmap/obstacles/base_scan/raytrace_range: 5.0
 * /move_base/global_costmap/obstacles/base_scan/topic: base_scan
 * /move_base/global_costmap/obstacles/max_obstacle_height: 2.0
 * /move_base/global_costmap/obstacles/observation_sources: base_scan
 * /move_base/global_costmap/obstacles/z_resolution: 0.125
 * /move_base/global_costmap/obstacles/z_voxels: 16
 * /move_base/global_costmap/plugins: [{'type': 'costma...
 * /move_base/global_costmap/publish_frequency: 0.0
 * /move_base/global_costmap/robot_base_frame: base_link
 * /move_base/global_costmap/robot_radius: 0.3
 * /move_base/global_costmap/static_map: True
 * /move_base/global_costmap/transform_tolerance: 0.5
 * /move_base/global_costmap/update_frequency: 5.0
 * /move_base/local_costmap/global_frame: odom
 * /move_base/local_costmap/height: 4.0
 * /move_base/local_costmap/inflater/inflation_radius: 0.7
 * /move_base/local_costmap/inflater/robot_radius: 0.3
 * /move_base/local_costmap/obstacles/base_scan/clearing: True
 * /move_base/local_costmap/obstacles/base_scan/data_type: LaserScan
 * /move_base/local_costmap/obstacles/base_scan/marking: True
 * /move_base/local_costmap/obstacles/base_scan/max_obstacle_height: 0.3
 * /move_base/local_costmap/obstacles/base_scan/min_obstacle_height: 0.25
 * /move_base/local_costmap/obstacles/base_scan/obstacle_range: 4.0
 * /move_base/local_costmap/obstacles/base_scan/raytrace_range: 5.0
 * /move_base/local_costmap/obstacles/base_scan/topic: base_scan
 * /move_base/local_costmap/obstacles/max_obstacle_height: 2.0
 * /move_base/local_costmap/obstacles/observation_sources: base_scan
 * /move_base/local_costmap/obstacles/publish_observations: False
 * /move_base/local_costmap/obstacles/z_resolution: 0.125
 * /move_base/local_costmap/obstacles/z_voxels: 16
 * /move_base/local_costmap/plugins: [{'type': 'costma...
 * /move_base/local_costmap/publish_frequency: 2.0
 * /move_base/local_costmap/resolution: 0.025
 * /move_base/local_costmap/robot_base_frame: base_link
 * /move_base/local_costmap/robot_radius: 0.3
 * /move_base/local_costmap/rolling_window: True
 * /move_base/local_costmap/transform_tolerance: 0.5
 * /move_base/local_costmap/width: 4.0
 * /move_base/oscillation_distance: 0.5
 * /move_base/oscillation_timeout: 10.0
 * /move_base/planner_frequency: 0.0
 * /move_base/planner_patience: 5.0
 * /move_base/recovery_behavior_enabled: True
 * /move_base/recovery_behaviors: [{'type': 'clear_...
 * /move_base/rotate_recovery/frequency: 20.0
 * /move_base/rotate_recovery/sim_granularity: 0.017
 * /move_group/allow_trajectory_execution: True
 * /move_group/allowed_execution_duration_scaling: 1.2
 * /move_group/allowed_goal_duration_margin: 0.5
 * /move_group/arm/longest_valid_segment_fraction: 0.05
 * /move_group/arm/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm/projection_evaluator: joints(shoulder_p...
 * /move_group/arm_with_torso/longest_valid_segment_fraction: 0.05
 * /move_group/arm_with_torso/planner_configs: ['SBLkConfigDefau...
 * /move_group/arm_with_torso/projection_evaluator: joints(torso_lift...
 * /move_group/capabilities: move_group/MoveGr...
 * /move_group/controller_list: [{'default': True...
 * /move_group/gripper/planner_configs: ['SBLkConfigDefau...
 * /move_group/jiggle_fraction: 0.05
 * /move_group/max_safe_path_cost: 1
 * /move_group/moveit_controller_manager: moveit_simple_con...
 * /move_group/moveit_manage_controllers: True
 * /move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE
 * /move_group/planner_configs/ESTkConfigDefault/type: geometric::EST
 * /move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE
 * /move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE
 * /move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM
 * /move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar
 * /move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...
 * /move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT
 * /move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar
 * /move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL
 * /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/start_state_max_bounds_error: 0.1
 * /robot_description_kinematics/arm/kinematics_solver: fetch_arm_kinemat...
 * /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_kinematics/arm_with_torso/kinematics_solver: kdl_kinematics_pl...
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm_with_torso/kinematics_solver_timeout: 0.005
 * /robot_description_planning/joint_limits/elbow_flex_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/elbow_flex_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/elbow_flex_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/elbow_flex_joint/max_velocity: 1.5
 * /robot_description_planning/joint_limits/forearm_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/forearm_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/forearm_roll_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/forearm_roll_joint/max_velocity: 1.57
 * /robot_description_planning/joint_limits/left_gripper_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/left_gripper_joint/has_velocity_limits: False
 * /robot_description_planning/joint_limits/left_gripper_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/left_gripper_joint/max_velocity: 0
 * /robot_description_planning/joint_limits/right_gripper_joint/has_acceleration_limits: False
 * /robot_description_planning/joint_limits/right_gripper_joint/has_velocity_limits: False
 * /robot_description_planning/joint_limits/right_gripper_joint/max_acceleration: 0
 * /robot_description_planning/joint_limits/right_gripper_joint/max_velocity: 0
 * /robot_description_planning/joint_limits/shoulder_lift_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/shoulder_lift_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_acceleration: 1.0
 * /robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity: 1.0
 * /robot_description_planning/joint_limits/shoulder_pan_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/shoulder_pan_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity: 1.256
 * /robot_description_planning/joint_limits/torso_lift_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/torso_lift_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/torso_lift_joint/max_acceleration: 0.5
 * /robot_description_planning/joint_limits/torso_lift_joint/max_velocity: 0.1
 * /robot_description_planning/joint_limits/upperarm_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/upperarm_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/upperarm_roll_joint/max_acceleration: 1.5
 * /robot_description_planning/joint_limits/upperarm_roll_joint/max_velocity: 1.57
 * /robot_description_planning/joint_limits/wrist_flex_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_flex_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_flex_joint/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/wrist_flex_joint/max_velocity: 2.26
 * /robot_description_planning/joint_limits/wrist_roll_joint/has_acceleration_limits: True
 * /robot_description_planning/joint_limits/wrist_roll_joint/has_velocity_limits: True
 * /robot_description_planning/joint_limits/wrist_roll_joint/max_acceleration: 2.5
 * /robot_description_planning/joint_limits/wrist_roll_joint/max_velocity: 2.26
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: melodic
 * /rosversion: 1.14.3

NODES
  /
    amcl (amcl/amcl)
    basic_grasping_perception (simple_grasping/basic_grasping_perception)
    demo (fetch_gazebo_demo/demo.py)
    map_server (map_server/map_server)
    move_base (move_base/move_base)
    move_group (moveit_ros_move_group/move_group)
    tilt_head_node (fetch_navigation/tilt_head.py)

ROS_MASTER_URI=http://localhost:11311

running rosparam delete /move_group/sensors
ERROR: parameter [/move_group/sensors] is not set
process[map_server-1]: started with pid [15744]
process[amcl-2]: started with pid [15745]
process[move_base-3]: started with pid [15751]
process[tilt_head_node-4]: started with pid [15754]
process[move_group-5]: started with pid [15758]
process[basic_grasping_perception-6]: started with pid [15764]
process[demo-7]: started with pid [15766]
[ INFO] [1550152816.255272910]: Loading robot model 'fetch'...
[ INFO] [1550152816.264961261]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1550152817.193265549, 38.395000000]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ WARN] [1550152817.226394094, 38.397000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1550152817.673541007, 38.430000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1550152817.685287940, 38.431000000]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1550152817.685621569, 38.431000000]: Starting planning scene monitor
[ INFO] [1550152817.693314597, 38.432000000]: Listening to '/planning_scene'
[ INFO] [1550152817.693583912, 38.432000000]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1550152817.706299892, 38.433000000]: Listening to '/collision_object' using message notifier with target frame 'base_link '
[ INFO] [1550152817.710822605, 38.433000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1550152817.711709211, 38.433000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1550152817.820010986, 38.443000000]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1550152817.986319777, 38.457000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1550152818.126242942, 38.461000000]: Using planning interface 'OMPL'
[ INFO] [1550152818.131157432, 38.461000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1550152818.132135258, 38.461000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1550152818.133559413, 38.462000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550152818.136698062, 38.462000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550152818.137947254, 38.462000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1550152818.139511116, 38.462000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1550152818.139873359, 38.462000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1550152818.139994481, 38.462000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1550152818.140084337, 38.462000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1550152818.140180906, 38.462000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1550152818.140330353, 38.462000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1550152818.445939430, 38.499000000]: Added FollowJointTrajectory controller for arm_controller
[ INFO] [1550152818.755643478, 38.530000000]: Added FollowJointTrajectory controller for arm_with_torso_controller
[ INFO] [1550152819.057558648, 38.558000000]: Added GripperCommand controller for gripper_controller
[ INFO] [1550152819.057960954, 38.558000000]: Returned 3 controllers in list
[ INFO] [1550152819.157349446, 38.564000000]: 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] [1550152819.604622392, 38.596000000]: 

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

[ INFO] [1550152819.604780873, 38.596000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1550152819.604842572, 38.596000000]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1550152821.238896216, 38.897000000]: Using plugin "static_map"
[ INFO] [1550152821.271485169, 38.898000000]: Requesting the map...
[ INFO] [1550152821.797832030, 38.999000000]: Resizing costmap to 401 X 444 at 0.050000 m/pix
[ INFO] [1550152822.318402372, 39.099000000]: Received a 401 X 444 map at 0.050000 m/pix
[ INFO] [1550152822.336528615, 39.102000000]: Using plugin "obstacles"
[INFO] [1550152822.717252, 39.164000]: Waiting for move_base...
[ INFO] [1550152822.724583598, 39.166000000]:     Subscribed to Topics: base_scan
[ INFO] [1550152822.868011344, 39.193000000]: Using plugin "inflater"
[ INFO] [1550152823.077351848, 39.230000000]: Using plugin "obstacles"
[ INFO] [1550152823.092972954, 39.234000000]:     Subscribed to Topics: base_scan
[ INFO] [1550152823.291826867, 39.275000000]: Using plugin "inflater"
[ INFO] [1550152823.465695819, 39.309000000]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1550152823.524523393, 39.323000000]: Sim period is set to 0.04
[ INFO] [1550152824.266918156, 39.506000000]: Recovery behavior will clear layer obstacles
[ INFO] [1550152824.302552200, 39.515000000]: Recovery behavior will clear layer obstacles
[ INFO] [1550152824.497807764, 39.554000000]: odom received!
[INFO] [1550152824.632219, 39.586000]: Waiting for torso_controller...
[INFO] [1550152824.819133, 39.625000]: Waiting for head_controller...
[INFO] [1550152825.082742, 39.690000]: Waiting for get_planning_scene
[INFO] [1550152825.118656, 39.698000]: Connecting to pickup action...
[INFO] [1550152825.435118, 39.776000]: ...connected
[INFO] [1550152825.437498, 39.776000]: Connecting to place action...
[INFO] [1550152825.670877, 39.824000]: ...connected
[INFO] [1550152825.917689, 39.862000]: Waiting for basic_grasping_perception/find_objects...
[INFO] [1550152826.167008, 39.894000]: Moving to table...
[ WARN] [1550152935.337158025, 67.535000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550152980.132707802, 77.615000000]: Rotate recovery behavior started.
[ WARN] [1550153058.423329389, 95.251000000]: Invalid Trajectory 0.093694, 0.000000, 0.025957, cost: -1.000000
[ WARN] [1550153093.262487593, 103.608000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153134.243491509, 113.969000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153178.759782706, 125.168000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153254.751576573, 143.775000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1550153327.259470285, 162.218000000]: Rotate recovery behavior started.
[ WARN] [1550153366.278706368, 172.399000000]: Clearing costmap to unstuck robot (0.500000m).
[ERROR] [1550153366.753323531, 172.518000000]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
[INFO] [1550153366.781150, 172.524000]: Done moving to table...
[INFO] [1550153366.785093, 172.526000]: Raising torso...
Get block to pick...
[INFO] [1550153391.572698, 178.801000]: Picking object...
[WARN] [1550153392.673143, 178.988000]: ObjectManager: object0 not added yet
[WARN] [1550153392.681189, 178.992000]: ObjectManager: object1 not added yet
[WARN] [1550153392.688479, 178.993000]: ObjectManager: object2 not added yet
[WARN] [1550153392.695956, 178.993000]: ObjectManager: surface0 not added yet
[WARN] [1550153392.702579, 178.996000]: ObjectManager: surface1 not added yet
[INFO] [1550153393.662212, 179.207000]: Beginning to pick.
[ INFO] [1550153393.710886642, 179.216000000]: Planning attempt 1 of at most 1
[ INFO] [1550153393.747439878, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153393.748480877, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153393.748852458, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153393.749536260, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153393.750028521, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153393.750879788, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550153393.751895365, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550153393.752869652, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550153393.753795769, 179.224000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550153393.754273214, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550153393.754462461, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550153393.754598282, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550153393.754749107, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550153393.754867200, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550153393.755051772, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153393.755189438, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550153393.755334915, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550153393.755452999, 179.225000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550153393.780708026, 179.227000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.786793545, 179.228000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153393.804705611, 179.231000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.808533135, 179.231000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.808729677, 179.231000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153393.815110041, 179.231000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153393.830177394, 179.234000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153393.830652548, 179.234000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.838803211, 179.235000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.844853167, 179.235000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.859924986, 179.237000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153393.863973966, 179.238000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153393.864321996, 179.238000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.872887149, 179.238000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153393.878539880, 179.239000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.881859741, 179.239000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153393.891787588, 179.239000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153393.891941139, 179.239000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 2
[ WARN] [1550153393.892547257, 179.239000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1550153393.892858277, 179.239000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153393.895074774, 179.239000000]: IK failed
[ INFO] [1550153393.896964234, 179.239000000]: IK failed
[ INFO] [1550153393.903994475, 179.239000000]: IK failed
[ INFO] [1550153393.904274886, 179.239000000]: Sampler failed to produce a state
[ INFO] [1550153393.904446527, 179.239000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153393.905806261, 179.239000000]: Pickup planning completed after 0.181321 seconds
[ERROR] [1550153393.940272, 179.246000]: Pick failed in the planning stage, try again...
[ INFO] [1550153396.107112117, 179.756000000]: Planning attempt 1 of at most 1
[ INFO] [1550153396.107461923, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153396.107523632, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153396.107551375, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550153396.107576767, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550153396.107599795, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550153396.107623829, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550153396.107646277, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550153396.107668341, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550153396.107692867, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550153396.107722079, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550153396.107759870, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550153396.107791824, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153396.107824336, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550153396.107865058, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550153396.107955347, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550153396.108011277, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550153396.108055433, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550153396.108102517, 179.756000000]: Added plan for pipeline 'pick'. Queue is now of size 18
[ INFO] [1550153396.118587283, 179.756000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.120641004, 179.756000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153396.124350701, 179.756000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.130405914, 179.756000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.133675837, 179.756000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153396.139437541, 179.756000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.154993244, 179.760000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153396.159013530, 179.760000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153396.159131229, 179.760000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.163847635, 179.761000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.171108347, 179.763000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.173989059, 179.763000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153396.178834195, 179.764000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153396.180172085, 179.764000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.187044569, 179.764000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153396.200858775, 179.768000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.201879521, 179.768000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153396.207803285, 179.768000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 1
[ WARN] [1550153396.210395812, 179.768000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1550153396.210934814, 179.768000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153396.213506176, 179.770000000]: IK failed
[ INFO] [1550153396.215496819, 179.770000000]: IK failed
[ INFO] [1550153396.218012068, 179.770000000]: IK failed
[ INFO] [1550153396.221173104, 179.771000000]: Sampler failed to produce a state
[ INFO] [1550153396.221560718, 179.771000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153396.224782682, 179.772000000]: Pickup planning completed after 0.103031 seconds
[ERROR] [1550153396.235715, 179.773000]: Pick failed in the planning stage, try again...
[ INFO] [1550153398.378370518, 180.284000000]: Planning attempt 1 of at most 1
[ INFO] [1550153398.378739755, 180.284000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153398.378807529, 180.284000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153398.382805380, 180.285000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153398.384706454, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153398.384853428, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550153398.384914989, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550153398.384955950, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550153398.384992605, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550153398.385033004, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550153398.385074768, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550153398.385121558, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550153398.385181272, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550153398.385233268, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550153398.385285612, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153398.385340240, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550153398.385391106, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550153398.385451967, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550153398.385513089, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550153398.385569822, 180.285000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550153398.403623454, 180.286000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.408771049, 180.286000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153398.410371499, 180.286000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.416666540, 180.288000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153398.420468852, 180.288000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153398.430312991, 180.290000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.436804121, 180.290000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153398.442740915, 180.290000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.447288611, 180.291000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.448312415, 180.292000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153398.448692486, 180.292000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153398.451544899, 180.292000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.453659137, 180.292000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153398.458705982, 180.292000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.469929670, 180.293000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153398.470030426, 180.293000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153398.480887883, 180.294000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 1
[ WARN] [1550153398.481222248, 180.294000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1550153398.481454257, 180.294000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153398.483513377, 180.294000000]: IK failed
[ INFO] [1550153398.489574325, 180.296000000]: IK failed
[ INFO] [1550153398.498495097, 180.297000000]: IK failed
[ INFO] [1550153398.499202991, 180.297000000]: Sampler failed to produce a state
[ INFO] [1550153398.500163399, 180.297000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153398.501141181, 180.298000000]: Pickup planning completed after 0.102676 seconds
[ERROR] [1550153398.509153, 180.298000]: Pick failed in the planning stage, try again...
[ INFO] [1550153400.571026984, 180.814000000]: Planning attempt 1 of at most 1
[ INFO] [1550153400.571347382, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153400.571399242, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550153400.571433898, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550153400.571470866, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550153400.571501954, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550153400.571534917, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550153400.571565750, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550153400.571599444, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550153400.571632666, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550153400.571665406, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550153400.571697967, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550153400.571732038, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153400.571767846, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550153400.571800534, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550153400.571831835, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550153400.571869107, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550153400.571898067, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550153400.571928476, 180.814000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550153400.586055423, 180.818000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.591401481, 180.818000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.601141103, 180.818000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.607128709, 180.819000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.608128440, 180.819000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153400.624258590, 180.820000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153400.626616510, 180.820000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.630235842, 180.820000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.639196048, 180.821000000]: Manipulation plan 10 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153400.649360544, 180.823000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153400.656643976, 180.823000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.657404738, 180.823000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.668111743, 180.826000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550153400.679405593, 180.826000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153400.687058933, 180.827000000]: Manipulation plan 13 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.690214022, 180.827000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550153400.694703205, 180.827000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550153400.696004253, 180.827000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 3
[ WARN] [1550153400.708756540, 180.830000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1550153400.708940172, 180.830000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550153400.711116214, 180.830000000]: IK failed
[ INFO] [1550153400.719230453, 180.830000000]: IK failed
[ INFO] [1550153400.722598089, 180.830000000]: IK failed
[ INFO] [1550153400.722787301, 180.830000000]: Sampler failed to produce a state
[ INFO] [1550153400.722872173, 180.830000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550153400.727197412, 180.830000000]: Pickup planning completed after 0.137375 seconds
[ERROR] [1550153400.736623, 180.835000]: Pick failed in the planning stage, try again...
^C

umhan35 avatar Feb 14 '19 14:02 umhan35

Sounds like the same issue reported here https://github.com/fetchrobotics/fetch_ros/issues/102

@narora1 @safrimus

moriarty avatar Feb 14 '19 15:02 moriarty

We've made some improvements but I think this is still a bit of an issue.

I've added Help Wanted and More Info Needed, similar to #36

moriarty avatar Apr 05 '19 21:04 moriarty

@umhan35 Hi, it might work better if you reduce the inflation_radius specified in costmap_common.yaml under fetch_navigation/config. (the default value is 0.7m) In your case, the robot has to be close to the table to pick up the cube, while large inflation_radius keeps the robot away from the obstacles, and the robot starts rotation recovery if it is within the inflation layer of the table.

slugflow avatar Jul 04 '19 01:07 slugflow

@xujingren We actually have another map and move_base config that works in our test course but not in Gazebo 9. The root cause is #102. We tested on Ubuntu 14.04 with Gazebo 2 and it works.

umhan35 avatar Jul 04 '19 02:07 umhan35