zebradevs / fetch_gazebo

Gazebo simulator for Fetch

Fetch Gazebo

This repository contains the Gazebo simulation for Fetch Robotics Fetch and Freight Research Edition Robots.

Please note the branch: the default branch in GitHub is gazebo9.

1. gazebo11 should be used with ROS Noetic and Ubuntu 20.04 (untested as of 1-2021)
2. gazebo9 should be used with ROS Melodic and Ubuntu 18.04
3. gazebo7 should be used with ROS Kinetic and Ubuntu 16.04 (not supported on hardware)
4. gazebo2 should be used with ROS Indigo and Ubuntu 14.04 (EOL)

Tutorial & Documentation

Please refer to our documentation page: http://docs.fetchrobotics.com/gazebo.html

Launch it on ROSDS

The Fetch Gazebo packages can be run in the cloud, through an external service ROSDS, provided by TheConstruct. http://docs.fetchrobotics.com/gazebo.html#launch-it-on-rosds

ROS Buildfarm Release

Fetch Gazebo Package	Kinetic Source	Kinetic Debian	Melodic Source	Melodic Debian
fetch_gazebo
fetch_gazebo_demo
fetchit_challenge

ROS Buildfarm Devel

Fetch Gazebo Package	Melodic Devel
fetch_gazebo
fetch_gazebo_demo
fetchit_challenge

fetch_gazebo's Issues

One error

In file included from /opt/ros/melodic/include/actionlib/server/handle_tracker_deleter.h:42:0,
                 from /opt/ros/melodic/include/actionlib/server/action_server.h:51,
                 from /opt/ros/melodic/include/actionlib/server/simple_action_server.h:42,
                 from /opt/ros/melodic/include/robot_controllers_interface/controller_manager.h:36,
                 from /home/zephyr/catkin_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp:39:
/opt/ros/melodic/include/actionlib/destruction_guard.h: In member function ‘void actionlib::DestructionGuard::destruct()’:
/opt/ros/melodic/include/actionlib/destruction_guard.h:62:80: error: no matching function for call to ‘boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000>::subsecond_duration(float)’
    count_condition_.timed_wait(lock, boost::posix_time::milliseconds(1000.0f));
                                                                             ^
In file included from /usr/local/include/boost/date_time/posix_time/posix_time_config.hpp:16:0,
                 from /usr/local/include/boost/date_time/posix_time/posix_time_system.hpp:13,
                 from /usr/local/include/boost/date_time/posix_time/ptime.hpp:12,
                 from /usr/local/include/boost/date_time/posix_time/posix_time_types.hpp:12,
                 from /usr/local/include/boost/thread/thread_time.hpp:11,
                 from /usr/local/include/boost/thread/detail/platform_time.hpp:11,
                 from /usr/local/include/boost/thread/pthread/condition_variable.hpp:9,
                 from /usr/local/include/boost/thread/condition_variable.hpp:16,
                 from /usr/local/include/boost/thread/condition.hpp:13,
                 from /opt/ros/melodic/include/actionlib/server/simple_action_server.h:40,
                 from /opt/ros/melodic/include/robot_controllers_interface/controller_manager.h:36,
                 from /home/zephyr/catkin_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp:39:
/usr/local/include/boost/date_time/time_duration.hpp:285:14: note: candidate: template<class T> boost::date_time::subsecond_duration<base_duration, frac_of_second>::subsecond_duration(const T&, typename boost::enable_if<boost::is_integral<Functor>, void>::type*)
     explicit subsecond_duration(T const& ss,
              ^~~~~~~~~~~~~~~~~~
/usr/local/include/boost/date_time/time_duration.hpp:285:14: note:   template argument deduction/substitution failed:
/usr/local/include/boost/date_time/time_duration.hpp: In substitution of ‘template<class T> boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000>::subsecond_duration(const T&, typename boost::enable_if<boost::is_integral<T> >::type*) [with T = float]’:
/opt/ros/melodic/include/actionlib/destruction_guard.h:62:80:   required from here
/usr/local/include/boost/date_time/time_duration.hpp:285:14: error: no type named ‘type’ in ‘struct boost::enable_if<boost::is_integral<float>, void>’
In file included from /usr/local/include/boost/date_time/posix_time/posix_time_config.hpp:16:0,
                 from /usr/local/include/boost/date_time/posix_time/posix_time_system.hpp:13,
                 from /usr/local/include/boost/date_time/posix_time/ptime.hpp:12,
                 from /usr/local/include/boost/date_time/posix_time/posix_time_types.hpp:12,
                 from /usr/local/include/boost/thread/thread_time.hpp:11,
                 from /usr/local/include/boost/thread/detail/platform_time.hpp:11,
                 from /usr/local/include/boost/thread/pthread/condition_variable.hpp:9,
                 from /usr/local/include/boost/thread/condition_variable.hpp:16,
                 from /usr/local/include/boost/thread/condition.hpp:13,
                 from /opt/ros/melodic/include/actionlib/server/simple_action_server.h:40,
                 from /opt/ros/melodic/include/robot_controllers_interface/controller_manager.h:36,
                 from /home/zephyr/catkin_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp:39:
/usr/local/include/boost/date_time/time_duration.hpp:270:30: note: candidate: boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000>::subsecond_duration(const boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000>&)
   class BOOST_SYMBOL_VISIBLE subsecond_duration : public base_duration
                              ^~~~~~~~~~~~~~~~~~
/usr/local/include/boost/date_time/time_duration.hpp:270:30: note:   no known conversion for argument 1 from ‘float’ to ‘const boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000>&’
/usr/local/include/boost/date_time/time_duration.hpp:270:30: note: candidate: boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000>::subsecond_duration(boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000>&&)
/usr/local/include/boost/date_time/time_duration.hpp:270:30: note:   no known conversion for argument 1 from ‘float’ to ‘boost::date_time::subsecond_duration<boost::posix_time::time_duration, 1000>&&’
CMakeFiles/fetch_gazebo_plugin.dir/build.make:62: recipe for target 'CMakeFiles/fetch_gazebo_plugin.dir/src/plugin.cpp.o' failed
make[2]: *** [CMakeFiles/fetch_gazebo_plugin.dir/src/plugin.cpp.o] Error 1
CMakeFiles/Makefile2:72: recipe for target 'CMakeFiles/fetch_gazebo_plugin.dir/all' failed
make[1]: *** [CMakeFiles/fetch_gazebo_plugin.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2

I can't understand this error at all, so any suggestions? Thanks a lot!

Add multi-robot support in Gazebo simulation

I am trying to add multiple Fetch robots in the gazebo environment, but it seems too difficult to make it work. Could you provide this feature? Thank you!

[FetchIt!] Additional Kits in the Gazebo World

How many kit caddies will be place on the kit station table? The simulation only has one but the rulebook says “Kits will be approximately evenly spaced and not touching”, which means there will be several of them. If that’s the case, could you modify the Gazebo world to reflect that?

@umhan35 asked via email.

I just checked the arena, and 3 fit nicely on a table to start.

After 2 have been processed (either finished and delivered or dropped), the kit table will be replenished. But in the Gazebo Model, I think 3 will be fine, as two complete deliveries are required for winning a Fetch.

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

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:

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

Problem: gazebo7 with joint_handle.h and plugin.cpp

Hello. Thanks for your code of FETCH.
And I run it in my computer with ubuntu16.04 and ROS kinetic, gazebo7.14.
Then I encounter some problem below, I think it belongs to the version of gazebo.

In file included from /home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp:40:0:
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/include/fetch_gazebo/joint_handle.h: In member function ‘virtual double gazebo::JointHandle::getPosition()’:
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/include/fetch_gazebo/joint_handle.h:131:46: error: ‘class gazebo::physics::Joint’ has no member named ‘Position’
       return angles::normalize_angle(joint_->Position(0));
                                              ^
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/include/fetch_gazebo/joint_handle.h:133:20: error: ‘class gazebo::physics::Joint’ has no member named ‘Position’
     return joint_->Position(0);
                    ^
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/include/fetch_gazebo/joint_handle.h: In member function ‘virtual double gazebo::JointHandle::getPositionMin()’:
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/include/fetch_gazebo/joint_handle.h:157:20: error: ‘class gazebo::physics::Joint’ has no member named ‘LowerLimit’
     return joint_->LowerLimit(0);
                    ^
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/include/fetch_gazebo/joint_handle.h: In member function ‘virtual double gazebo::JointHandle::getPositionMax()’:
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/include/fetch_gazebo/joint_handle.h:163:20: error: ‘class gazebo::physics::Joint’ has no member named ‘UpperLimit’
     return joint_->UpperLimit(0);
                    ^
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp: In member function ‘virtual void gazebo::FetchGazeboPlugin::Init()’:
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp:97:40: error: ‘class gazebo::physics::World’ has no member named ‘SimTime’
   prevUpdateTime = model_->GetWorld()->SimTime();
                                        ^
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp:110:71: error: conversion from ‘urdf::JointConstSharedPtr {aka boost::shared_ptr<const urdf::Joint>}’ to non-scalar type ‘std::shared_ptr<const urdf::Joint>’ requested
     std::shared_ptr<const urdf::Joint> urdf_joint = urdfmodel.getJoint((*it)->GetName());
                                                                       ^
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp: In member function ‘void gazebo::FetchGazeboPlugin::OnUpdate(const gazebo::common::UpdateInfo&)’:
/home/zzl/fetch_ws/src/fetch_gazebo/fetch_gazebo/src/plugin.cpp:138:47: error: ‘class gazebo::physics::World’ has no member named ‘SimTime’
   common::Time currTime = model_->GetWorld()->SimTime();
                                               ^
fetch_gazebo/fetch_gazebo/CMakeFiles/fetch_gazebo_plugin.dir/build.make:62: recipe for target 'fetch_gazebo/fetch_gazebo/CMakeFiles/fetch_gazebo_plugin.dir/src/plugin.cpp.o' failed
make[2]: *** [fetch_gazebo/fetch_gazebo/CMakeFiles/fetch_gazebo_plugin.dir/src/plugin.cpp.o] Error 1
CMakeFiles/Makefile2:2965: recipe for target 'fetch_gazebo/fetch_gazebo/CMakeFiles/fetch_gazebo_plugin.dir/all' failed
make[1]: *** [fetch_gazebo/fetch_gazebo/CMakeFiles/fetch_gazebo_plugin.dir/all] Error 2
Makefile:138: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

Release for -Kinetic- (missed) -> Melodic

Dependent repositories still to be released into Jade:

• grasping_msgs
• sparse_bundle_adjustment
• open_karto
• slam_karto
• robot_controllers
• robot_calibration (upstream blocker: mikeferguson/robot_calibration#29)
• fetch_ros (fetch_description, fetch_navigation, fetch_moveit_config)
  • depends on ZebraDevs/fetch_ros#52 being merged
• simple_grasping
• moveit_python (upstream blocker: mikeferguson/moveit_python#8)
• moveit

Fixes to fetch_gazebo (gazebo7 branch)

• update package.xml depends for gazebo 7
• find missing depend for making laser be lasery

Possible improvements:

• use new friction models to improve simulation

Release for Melodic

We missed #6 and #15 but we will release for Melodic

And @velveteenrobot is working on an environment for our competition at ICRA.
https://discourse.ros.org/t/announcement-win-a-robot-fetchit-the-fetch-mobile-manipulation-challenge-icra-2019-competition/7569

Adding namespace to Fetch Robot in Gazebo

Hi team,

I am trying to create a world in Gazebo comprising of multiple robots and for which I need to add namespace to Fetch robot.
Please help me with the steps/ procedure for same.

I am using gazebo7 with ROS Kinetic and Ubuntu 16.04

Thanks and Regards,
Saloni

About the demo.py in fetch_gazebo_demo

When the simulation environment got ready, we ran the demo.py. And after the robot arriving the first table, errors occurred. It look likes this

`Traceback (most recent call last):
File "/home/lwx/fetch_ws/src/fetch_tc/fetch_sim/fetch_gazebo_demo/scripts/demo.py", line 276, in
grasping_client.updateScene()
File "/home/lwx/fetch_ws/src/fetch_tc/fetch_sim/fetch_gazebo_demo/scripts/demo.py", line 148, in updateScene
for obj in find_result.objects:
AttributeError: 'NoneType' object has no attribute 'objects'

It seems that the robot could not find the cube. I sincerely hope someone can help us! Thanks a lot!

Controllers in Fetch Gazebo Melodic don't start correctly several times before the robot gets in a usable state

We have been using the Fetch Gazebo model in a Ubuntu 18.04 machine with ROS Melodic on it (Gazebo 9). We notice that when we spawn the robot in Gazebo, it takes several tries before the robot starts properly (controller kicked in, arm in a collision-free pose). More often than not, 90% of the times, the controllers don't start at all and the arm falls all the way through the robot base, getting in a stuck pose

We eventually get the robot to load properly and use it in Gazebo, but this controllers-not-starting initial situation takes up too much time. Is this a normal occurrence or are there any ideas of why this might be happening?

[FetchIt!] update the colours of the parts

Is your feature request related to a problem? Please describe.
Currently the large and small gear are green and red.

Describe the solution you'd like
All parts should be grey or blue, or have the option to test with both grey and blue.
Because that's the colour of the 3D print material we have on hand.

Describe alternatives you've considered
We don't mind the option for blue and grey. It would give us the flexibility in the future to say the kit needs to be assembled using all matching colours, but for this year we're most likely going to use all one colour.

Additional context
I'll add a picture of the parts we've printed so far

Dockerfiles for fetch_gazebo

This issue is more of a todo, I have a dockerfile which I've been using to test/debug #37 and should clean it up, and put it into this repository somewhere with a ReadMe and some instructions.

It can be passed arguments so that it works for Indigo/Kinetic/Melodic.

But it requires nvidia-docker2

ARG OPENGL_TAG=1.0-glvnd-devel-ubuntu16.04

FROM nvidia/opengl:${OPENGL_TAG}

ARG ROS_DISTRO
ARG GAZEBO_VERSION
ARG FETCH_ROS_VERSION

ENV ROS_DISTRO ${ROS_DISTRO:-kinetic}
ENV GAZEBO_VERSION ${GAZEBO_VERSION:-7}
ENV FETCH_ROS_VERSION ${FETCH_ROS_VERSION:-0.7.14}

# setup environment
ENV TERM xterm
ENV LANG C.UTF-8
ENV LC_ALL C.UTF-8

# nvidia-container-runtime
ENV NVIDIA_VISIBLE_DEVICES \
    ${NVIDIA_VISIBLE_DEVICES:-all}
ENV NVIDIA_DRIVER_CAPABILITIES \
    ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics

# install packages
RUN DEBIAN_FRONTEND=noninteractive apt-get update && \
    apt-get install -q -y \
      dirmngr \
      gnupg2 \
      lsb-release && \
    rm -rf /var/lib/apt/lists/*

# setup keys
RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 \
      --recv-keys 421C365BD9FF1F717815A3895523BAEEB01FA116

# setup sources.list
RUN echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > \
      /etc/apt/sources.list.d/ros-latest.list

# install bootstrap tools
RUN DEBIAN_FRONTEND=noninteractive apt-get update && \
    apt-get install --no-install-recommends -y \
      python-rosdep \
      python-rosinstall \
      python-vcstools && \
    rm -rf /var/lib/apt/lists/*

# bootstrap rosdep
RUN rosdep init \
    && rosdep update

# install ros packages
RUN DEBIAN_FRONTEND=noninteractive apt-get update && \
    apt-get install -y \
      build-essential \ 
      git \
      ros-${ROS_DISTRO}-desktop-full \
      python-catkin-tools \
      vim && \
    rm -rf /var/lib/apt/lists/*

RUN mkdir -p ros/${ROS_DISTRO}/src && \
    cd ros/${ROS_DISTRO} && \
    catkin config --init && \
    catkin config --extend /opt/ros/${ROS_DISTRO} && \
    cd src && \
    git clone --branch gazebo${GAZEBO_VERSION} https://github.com/fetchrobotics/fetch_gazebo.git && \
    git clone --branch ${FETCH_ROS_VERSION} https://github.com/fetchrobotics/fetch_ros.git && \
    cd ../ && \
    DEBIAN_FRONTEND=noninteractive apt-get update && \
    rosdep install -y --from-paths src --ignore-src \
      --rosdistro ${ROS_DISTRO} && \
    rm -rf /var/lib/apt/lists/*

RUN cd /ros/${ROS_DISTRO} && \
    catkin_make_isolated

fetch robot is dark in gazebo simulation playground

Describe the bug
Fetch robot is dark in the gazebo simulated playground.

To Reproduce
ROS Melodic, Ubuntu 18.04, Gazebo9

roslaunch fetch_gazebo playground.launch

Expected behavior
robot should not be dark

Screenshots

catkin workspace (please complete the following information):

• fetch_gazebo version: Gazebo 9
• Ubuntu version: 18.04
• ROS version: Melodic
• current catkin workspace information wstool info

Additional context
None

Using gazebo7 with the Fetch's gazebo?

Hi @mikeferguson et al,

Do you know if there is a way to install Fetch's gazebo stuff with gazebo 7? I have the usual Fetch setup with ROS Indigo and Ubuntu 14.04, but I was asking because I'm trying to use some code that's shared among different robots, and for the test worlds that we're using, we need gazebo 7 (since the "SDF" files have to be version 1.6, not 1.4 that I'm getting with gazebo2 now).

I was just inquiring because if I do a sudo apt-get install ros-indigo-fetch-gazebo-demo or sudo apt-get install ros-indigo-fetch-gazebo, these commands require gazebo2 as a dependency, BUT I noticed this repository has a gazebo7 branch. Is it possible to use that version of the fetch simulator, somehow?

Construct sim with fetch throws an exception

I tried getting fetch working on 18.04, melodic and with gazebo 9 but got this exception.

Also is there a way I can make edits to the fetch robot model ?

fetch_gazebo should configure cartesian twist controller

Hi. Is there any documentation for enabling & using the cartesian_twist_controller in gazebo to control the Fetch's manipulator?

I tried to add it to the fetch_gazebo/config/default_controllers.yaml file:

cartesian_twist_controller:
type: "robot_controllers/CartesianTwistController"
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- upperarm_roll_joint
- elbow_flex_joint
- forearm_roll_joint
- wrist_flex_joint
- wrist_roll_joint

and added it to the list of the gazebo/default_controllers.

If I run the start_controller.py script:

% ./start_controller.py cartesian_twist_controller
[INFO] [WallTime: 1502395440.391452] [0.000000] Connecting to /query_controller_states...
[INFO] [WallTime: 1502395440.721294] [147.571000] Done.
[INFO] [WallTime: 1502395440.721661] [147.571000] Requesting that cartesian_twist_controller be started...
[INFO] [WallTime: 1502395440.725579] [147.572000] Done.
%

but in the shell where gazebo was launched I see:

[ INFO] [1502395440.722918140, 147.571000000]: Stopped arm_controller/follow_joint_trajectory
[ INFO] [1502395440.723480279, 147.571000000]: Started cartesian_twist_controller
[ INFO] [1502395440.726726733, 147.572000000]: Stopped cartesian_twist_controller

In Gazebo the simulated arm then collapses, presumably because gravity compensation has been disabled. Running the get_controller_state.py script shows the twist controller as stopped:

% ./get_controller_state.py
[INFO] [WallTime: 1502395379.218454] [0.000000] Connecting to /query_controller_states...
[INFO] [WallTime: 1502395379.585420] [104.226000] Requesting state of controllers...
arm_controller/follow_joint_trajectory[robot_controllers/FollowJointTrajectoryController]: RUNNING
arm_controller/gravity_compensation[robot_controllers/GravityCompensation]: RUNNING
arm_with_torso_controller/follow_joint_trajectory[robot_controllers/FollowJointTrajectoryController]: STOPPED
base_controller[robot_controllers/DiffDriveBaseController]: RUNNING
head_controller/follow_joint_trajectory[robot_controllers/FollowJointTrajectoryController]: RUNNING
head_controller/point_head[robot_controllers/PointHeadController]: STOPPED
torso_controller/follow_joint_trajectory[robot_controllers/FollowJointTrajectoryController]: STOPPED
gripper_controller/gripper_action[robot_controllers/ParallelGripperController]: RUNNING
bellows_controller[robot_controllers/ScaledMimicController]: RUNNING
cartesian_pose_controller[robot_controllers/CartesianPoseController]: STOPPED
cartesian_twist_controller[robot_controllers/CartesianTwistController]: STOPPED

So what do I need to do to make this work?

Also will the same procedure enable the cartesian_twist_controller on a Fetch robot?

thanks,

-- Ron --

[FetchIt!] Modify chuck on Schunk machine

Background:
The real Schunk machine has an actuated chuck with jaws that can be opened and closed.
There is a ROS Node running on the network with a SimpleActionServer. You can publish a Goal msg to open and close the chuck.

This functionality needs to be added to the simulation.

This video shows you the pneumatic chuck with the moving jaws:
https://www.youtube.com/watch?v=Cl5XYM2xK1E
When the jaws are open, you can slide in your part.
Then you close the jaws and part stays there.

.

Tasks required:

• Which model number is the chuck?
  https://schunk.com/us_en/clamping-technology/series/rota-tp/
  Then we can download the CAD model directly from Schunk.

• What is that black part on the chuck?
  We can't find it on the above website.

• Make a new Collada 3D model of spindle, chuck and jaws with separate parts.
  It should be similar to this model:
  fetch_gazebo/fetchit_challenge/models/shunk_machine/meshes/shunk_machine_lathe_centered.dae
  but with the 3 jaws removed.

• Modify URDF of Schunk Machine.
  There is one link for the body of the machine.
  There is another link for the spindle + chuck.
  And 3 links for the 3 jaws, each rotated by 120 degrees. The Collada model for one jaw can be used 3 times.

• Make a simple joint controller.
  We can move the 3 joints together using a simple controller,
  or maybe hack some "mimic joint" like this:
  https://answers.ros.org/question/283537/how-to-do-mimic-joints-that-work-in-gazebo/

• Modify code in the SimpleActionServer:
  fetch_gazebo/fetchit_challenge/nodes/schunk_machine_server.py
  If you launch it with the sim flag, it will relay commands to the Gazebo controller.
  Otherwise it sends command to the GPIO port (the real hardware).

.

[FetchIt!] [BUG] RobotStatePublisher for Schunk machine breaks robot's /odom transform

Description:
You have defined the Schunk machine as a second robot with its own robot_state_publisher. However, this is not configured correctly and it breaks the TF tree for the Fetch robot's /base_link to /odom transform.

I'm not sure why this was working previously, but it should not have been.

Steps to reproduce:
Launch the simulation environment:

$ roslaunch fetchit_challenge main_arena_montreal2019_highlights.launch

Check this TF transform:

$ rosrun tf tf_echo /base_link /odom

You should see:

At time 0.0
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

where the timestamp is stuck at zero.
Also, if you generate the diagram of TF frames, you can see that the Schunk machine is connected directly to the robot's /base_link, which is incorrect.

Then, remove the robot_state_publisher in shunk_machine_control.launch and repeat.
You should see the correct output:

At time 9.537
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

where the timestamp is changing.

Screenshot:
This shows the TF tree with bad connection:

Expected behavior:
Each robot can have a /base_link but the TF frames need to be namespaced, like:

/fetch_robot1/base_link/
/schunk_machine/base_link/

and then the two robots are connected in the TF tree via the /map frame.

See the example in the first image on this page:
https://answers.ros.org/question/258948/why-tf-trees-for-multi-robot-system-not-showing/

Or you could try a hard-coded approach and write a simple node that publishes a static TF with the Schunk's location.

System configuration:

• fetch_gazebo version: Latest master branch
• Ubuntu version: 18.04
• ROS version: Melodic

.

Indigo & Kinetic fetch-gazebo backports.

This is just a TODO:

Although we don't support Kinetic on the Hardware, because we have not tested the drivers on the hardware, I've decided to release this to kinetic and backport all the changes for the FetchIt! Challenge to Indigo & Kinetic to make it more accessible.

ros/rosdistro#20726
ros/rosdistro#20725

fetchit_challenge files are not installed

I've just tested with catkin_make_isolated --install and found the fetchit_challenge package wasn't setup to install any of the files, and the package.xml is missing a dependency.

I was testing #51 before running bloom.

I've opened #56 to deal with some other issues.

[FetchIt!] Update SCHUNK Machine

@dekent has emailed asking about the SCHUNK Machine.

It was originally added to Gazebo after we only saw the CAD files, and didn't know how it actually looked but we knew it had a Logo on it.

Here is a photo of the actual machine, next to a shelf.
The Shelves will have other logos on them, from SICK, EandM, Fetch and The Construct Sim.

This was one of our old demo warehouse shelves. I've cut it down to match the height of the Schunk machine.

They can be disassembled and then they flat-pack for shipping, so we will use the same shelves in Montreal.

We don't have hard covers for the sides yet, but lets assume they'll look similar to the Schunk machine. They will be made out of some stiff material (likely foam-core board) and attach sides with "velcro".
The other competition co-sponsors will place their logos on these shelf sides.

I just took a photo of the back to show that it was a shelf. But the version for gazebo does not need to model the inside, it can simply be a solid rectangle.

Originally posted by @moriarty in #32 (comment)

[BUG] demo.launch will fail with Exception

Describe the bug
I launched demo.py but Fetch in gazebo will get stuck in the front of the table and finally fail with TypeError.

Fetch rotating in the front of the table when getting stuck

When demo.py failling with TypeError in the end, Fetch raise its torso.

Error which I got.

[ERROR] [WallTime: 1606829042.358124593, 367.401000000] [node:/move_base] [func:MoveBase::executeCycle]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
[INFO] [WallTime: 1606829042.392132, 367.425000] [node:/demo] [func:<module>]: Raising torso...
[INFO] [WallTime: 1606829050.088133, 373.459000] [node:/demo] [func:<module>]: Picking object...
Traceback (most recent call last):
  File "/opt/ros/melodic/lib/fetch_gazebo_demo/demo.py", line 274, in <module>
    grasping_client.updateScene()
  File "/opt/ros/melodic/lib/fetch_gazebo_demo/demo.py", line 152, in updateScene
    wait = False)
TypeError: addSolidPrimitive() got an unexpected keyword argument 'wait'
[demo-7] process has died [pid 4420, exit code 1, cmd /opt/ros/melodic/lib/fetch_gazebo_demo/demo.py __name:=demo __log:=/home/sktometometo/.ros/log/504a1e90-33d7-11eb-ac21-04ed33eed89c/demo-7.log].
log file: /home/sktometometo/.ros/log/504a1e90-33d7-11eb-ac21-04ed33eed89c/demo-7*.log

To Reproduce
The steps to help us reproduce this error.
We recommend using Docker to ensure your setup is as clean as possible, or using a new catkin workspace.

My environment is Ubuntu 18.04 + ROS melodic on Thinkpad T490.
fetch_gazebo and fetch_gazebo_demo is installed with apt.
The versions are below

/opt/ros/melodic/share/fetch_gazebo $ cat package.xml 
<?xml version="1.0"?>
<package format="2">
  <name>fetch_gazebo</name>
  <version>0.9.2</version>
  <description>
    Gazebo package for Fetch.
  </description>
  <author>Michael Ferguson</author>
  <maintainer email="[email protected]">Alex Moriarty</maintainer>
  <maintainer email="[email protected]">Niharika Arora</maintainer>
  <maintainer email="[email protected]">Sarah Elliott</maintainer>
  <license>BSD</license>
  <url>http://ros.org/wiki/fetch_gazebo</url>

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>angles</build_depend>
  <build_depend>gazebo_dev</build_depend>

  <depend>control_toolbox</depend>
  <depend>boost</depend>
  <depend>gazebo_plugins</depend>
  <depend>gazebo_ros</depend>
  <depend>robot_controllers</depend>
  <depend>robot_controllers_interface</depend>
  <depend>sensor_msgs</depend>

  <exec_depend>actionlib</exec_depend>
  <exec_depend>control_msgs</exec_depend>
  <exec_depend>depth_image_proc</exec_depend>
  <exec_depend>fetch_description</exec_depend>
  <exec_depend>gazebo</exec_depend>
  <exec_depend>image_proc</exec_depend>
  <exec_depend>nodelet</exec_depend>
  <exec_depend>rgbd_launch</exec_depend>
  <exec_depend>trajectory_msgs</exec_depend>
  <exec_depend>xacro</exec_depend>
</package>

/opt/ros/melodic/share/fetch_gazebo_demo $ cat package.xml 
<package>
  <name>fetch_gazebo_demo</name>
  <version>0.9.2</version>
  <description>
    Demos for fetch_gazebo package.
  </description>
  <author>Michael Ferguson</author>
  <maintainer email="[email protected]">Alex Moriarty</maintainer>
  <maintainer email="[email protected]">Niharika Arora</maintainer>
  <maintainer email="[email protected]">Sarah Elliott</maintainer>
  <license>BSD</license>
  <url>http://ros.org/wiki/fetch_gazebo_demo</url>
  
  <buildtool_depend>catkin</buildtool_depend>

  <run_depend>actionlib</run_depend>
  <run_depend>fetch_gazebo</run_depend>
  <run_depend>fetch_moveit_config</run_depend>
  <run_depend>fetch_navigation</run_depend>
  <run_depend>moveit_commander</run_depend>
  <run_depend>moveit_python</run_depend>
  <run_depend>simple_grasping</run_depend>
  <run_depend>teleop_twist_keyboard</run_depend>
</package>

I have launched demo.launch in fetch_gazebo_demo and playground.launch in fetch_gazebo

~ $ roslaunch fetch_gazebo playground.launch 
... logging to /home/sktometometo/.ros/log/504a1e90-33d7-11eb-ac21-04ed33eed89c/roslaunch-Persing-3753.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro.py is deprecated; please use xacro instead
started roslaunch server http://Persing:43709/

SUMMARY
========

PARAMETERS
 * /arm_controller/follow_joint_trajectory/joints: ['shoulder_pan_jo...
 * /arm_controller/follow_joint_trajectory/type: robot_controllers...
 * /arm_controller/gravity_compensation/autostart: True
 * /arm_controller/gravity_compensation/root: torso_lift_link
 * /arm_controller/gravity_compensation/tip: gripper_link
 * /arm_controller/gravity_compensation/type: robot_controllers...
 * /arm_with_torso_controller/follow_joint_trajectory/joints: ['torso_lift_join...
 * /arm_with_torso_controller/follow_joint_trajectory/type: robot_controllers...
 * /base_controller/autostart: True
 * /base_controller/type: robot_controllers...
 * /bellows_controller/autostart: True
 * /bellows_controller/controlled_joint: bellows_joint
 * /bellows_controller/mimic_joint: torso_lift_joint
 * /bellows_controller/mimic_scale: 0.5
 * /bellows_controller/type: robot_controllers...
 * /gazebo/bellows_joint/position/d: 0.0
 * /gazebo/bellows_joint/position/i: 0.0
 * /gazebo/bellows_joint/position/i_clamp: 0.0
 * /gazebo/bellows_joint/position/p: 10.0
 * /gazebo/bellows_joint/velocity/d: 0.0
 * /gazebo/bellows_joint/velocity/i: 0.0
 * /gazebo/bellows_joint/velocity/i_clamp: 0.0
 * /gazebo/bellows_joint/velocity/p: 25.0
 * /gazebo/default_controllers: ['arm_controller/...
 * /gazebo/elbow_flex_joint/position/d: 0.1
 * /gazebo/elbow_flex_joint/position/i: 0.0
 * /gazebo/elbow_flex_joint/position/i_clamp: 0.0
 * /gazebo/elbow_flex_joint/position/p: 100.0
 * /gazebo/elbow_flex_joint/velocity/d: 0.0
 * /gazebo/elbow_flex_joint/velocity/i: 0.0
 * /gazebo/elbow_flex_joint/velocity/i_clamp: 0.0
 * /gazebo/elbow_flex_joint/velocity/p: 150.0
 * /gazebo/enable_ros_network: True
 * /gazebo/forearm_roll_joint/position/d: 0.1
 * /gazebo/forearm_roll_joint/position/i: 0.0
 * /gazebo/forearm_roll_joint/position/i_clamp: 0.0
 * /gazebo/forearm_roll_joint/position/p: 100.0
 * /gazebo/forearm_roll_joint/velocity/d: 0.0
 * /gazebo/forearm_roll_joint/velocity/i: 0.0
 * /gazebo/forearm_roll_joint/velocity/i_clamp: 0.0
 * /gazebo/forearm_roll_joint/velocity/p: 150.0
 * /gazebo/head_pan_joint/position/d: 0.0
 * /gazebo/head_pan_joint/position/i: 0.0
 * /gazebo/head_pan_joint/position/i_clamp: 0.0
 * /gazebo/head_pan_joint/position/p: 2.0
 * /gazebo/head_pan_joint/velocity/d: 0.0
 * /gazebo/head_pan_joint/velocity/i: 0.0
 * /gazebo/head_pan_joint/velocity/i_clamp: 0.0
 * /gazebo/head_pan_joint/velocity/p: 2.0
 * /gazebo/head_tilt_joint/position/d: 0.0
 * /gazebo/head_tilt_joint/position/i: 0.0
 * /gazebo/head_tilt_joint/position/i_clamp: 0.0
 * /gazebo/head_tilt_joint/position/p: 10.0
 * /gazebo/head_tilt_joint/velocity/d: 0.0
 * /gazebo/head_tilt_joint/velocity/i: 0.0
 * /gazebo/head_tilt_joint/velocity/i_clamp: 0.0
 * /gazebo/head_tilt_joint/velocity/p: 3.0
 * /gazebo/l_gripper_finger_joint/position/d: 0.0
 * /gazebo/l_gripper_finger_joint/position/i: 0.0
 * /gazebo/l_gripper_finger_joint/position/i_clamp: 0.0
 * /gazebo/l_gripper_finger_joint/position/p: 5000.0
 * /gazebo/l_gripper_finger_joint/velocity/d: 0.0
 * /gazebo/l_gripper_finger_joint/velocity/i: 0.0
 * /gazebo/l_gripper_finger_joint/velocity/i_clamp: 0.0
 * /gazebo/l_gripper_finger_joint/velocity/p: 0.0
 * /gazebo/l_wheel_joint/position/d: 0.0
 * /gazebo/l_wheel_joint/position/i: 0.0
 * /gazebo/l_wheel_joint/position/i_clamp: 0.0
 * /gazebo/l_wheel_joint/position/p: 0.0
 * /gazebo/l_wheel_joint/velocity/d: 0.0
 * /gazebo/l_wheel_joint/velocity/i: 0.5
 * /gazebo/l_wheel_joint/velocity/i_clamp: 6.0
 * /gazebo/l_wheel_joint/velocity/p: 8.85
 * /gazebo/r_gripper_finger_joint/position/d: 0.0
 * /gazebo/r_gripper_finger_joint/position/i: 0.0
 * /gazebo/r_gripper_finger_joint/position/i_clamp: 0.0
 * /gazebo/r_gripper_finger_joint/position/p: 5000.0
 * /gazebo/r_gripper_finger_joint/velocity/d: 0.0
 * /gazebo/r_gripper_finger_joint/velocity/i: 0.0
 * /gazebo/r_gripper_finger_joint/velocity/i_clamp: 0.0
 * /gazebo/r_gripper_finger_joint/velocity/p: 0.0
 * /gazebo/r_wheel_joint/position/d: 0.0
 * /gazebo/r_wheel_joint/position/i: 0.0
 * /gazebo/r_wheel_joint/position/i_clamp: 0.0
 * /gazebo/r_wheel_joint/position/p: 0.0
 * /gazebo/r_wheel_joint/velocity/d: 0.0
 * /gazebo/r_wheel_joint/velocity/i: 0.5
 * /gazebo/r_wheel_joint/velocity/i_clamp: 6.0
 * /gazebo/r_wheel_joint/velocity/p: 8.85
 * /gazebo/shoulder_lift_joint/position/d: 0.1
 * /gazebo/shoulder_lift_joint/position/i: 0.0
 * /gazebo/shoulder_lift_joint/position/i_clamp: 0.0
 * /gazebo/shoulder_lift_joint/position/p: 100.0
 * /gazebo/shoulder_lift_joint/velocity/d: 0.0
 * /gazebo/shoulder_lift_joint/velocity/i: 0.0
 * /gazebo/shoulder_lift_joint/velocity/i_clamp: 0.0
 * /gazebo/shoulder_lift_joint/velocity/p: 200.0
 * /gazebo/shoulder_pan_joint/position/d: 0.1
 * /gazebo/shoulder_pan_joint/position/i: 0.0
 * /gazebo/shoulder_pan_joint/position/i_clamp: 0.0
 * /gazebo/shoulder_pan_joint/position/p: 100.0
 * /gazebo/shoulder_pan_joint/velocity/d: 0.0
 * /gazebo/shoulder_pan_joint/velocity/i: 2.0
 * /gazebo/shoulder_pan_joint/velocity/i_clamp: 1.0
 * /gazebo/shoulder_pan_joint/velocity/p: 200.0
 * /gazebo/torso_lift_joint/position/d: 0.0
 * /gazebo/torso_lift_joint/position/i: 0.0
 * /gazebo/torso_lift_joint/position/i_clamp: 0.0
 * /gazebo/torso_lift_joint/position/p: 1000.0
 * /gazebo/torso_lift_joint/velocity/d: 0.0
 * /gazebo/torso_lift_joint/velocity/i: 0.0
 * /gazebo/torso_lift_joint/velocity/i_clamp: 0.0
 * /gazebo/torso_lift_joint/velocity/p: 100000.0
 * /gazebo/upperarm_roll_joint/position/d: 0.1
 * /gazebo/upperarm_roll_joint/position/i: 0.0
 * /gazebo/upperarm_roll_joint/position/i_clamp: 0.0
 * /gazebo/upperarm_roll_joint/position/p: 100.0
 * /gazebo/upperarm_roll_joint/velocity/d: 0.0
 * /gazebo/upperarm_roll_joint/velocity/i: 0.0
 * /gazebo/upperarm_roll_joint/velocity/i_clamp: 0.0
 * /gazebo/upperarm_roll_joint/velocity/p: 10.0
 * /gazebo/wrist_flex_joint/position/d: 0.1
 * /gazebo/wrist_flex_joint/position/i: 0.0
 * /gazebo/wrist_flex_joint/position/i_clamp: 0.0
 * /gazebo/wrist_flex_joint/position/p: 100.0
 * /gazebo/wrist_flex_joint/velocity/d: 0.0
 * /gazebo/wrist_flex_joint/velocity/i: 0.0
 * /gazebo/wrist_flex_joint/velocity/i_clamp: 0.0
 * /gazebo/wrist_flex_joint/velocity/p: 100.0
 * /gazebo/wrist_roll_joint/position/d: 0.1
 * /gazebo/wrist_roll_joint/position/i: 0.0
 * /gazebo/wrist_roll_joint/position/i_clamp: 0.0
 * /gazebo/wrist_roll_joint/position/p: 100.0
 * /gazebo/wrist_roll_joint/velocity/d: 0.0
 * /gazebo/wrist_roll_joint/velocity/i: 0.0
 * /gazebo/wrist_roll_joint/velocity/i_clamp: 0.0
 * /gazebo/wrist_roll_joint/velocity/p: 100.0
 * /gripper_controller/gripper_action/centering/d: 0.0
 * /gripper_controller/gripper_action/centering/i: 0.0
 * /gripper_controller/gripper_action/centering/i_clamp: 0.0
 * /gripper_controller/gripper_action/centering/p: 1000.0
 * /gripper_controller/gripper_action/type: robot_controllers...
 * /head_camera/crop_decimate/decimation_x: 4
 * /head_camera/crop_decimate/decimation_y: 4
 * /head_camera/crop_decimate/queue_size: 1
 * /head_camera/head_camera_nodelet_manager/num_worker_threads: 2
 * /head_controller/follow_joint_trajectory/joints: ['head_pan_joint'...
 * /head_controller/follow_joint_trajectory/type: robot_controllers...
 * /head_controller/point_head/type: robot_controllers...
 * /publish_base_scan_raw/lazy: True
 * /robot/serial: ABCDEFGHIJKLMNOPQ...
 * /robot/version: 0.0.1
 * /robot_description: <?xml version="1....
 * /robot_state_publisher/publish_frequency: 100.0
 * /rosdistro: melodic
 * /rosversion: 1.14.10
 * /torso_controller/follow_joint_trajectory/joints: ['torso_lift_joint']
 * /torso_controller/follow_joint_trajectory/type: robot_controllers...
 * /use_sim_time: True

NODES
  /
    cmd_vel_mux (topic_tools/mux)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    prepare_robot (fetch_gazebo/prepare_simulated_robot.py)
    publish_base_scan_raw (topic_tools/relay)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    urdf_spawner (gazebo_ros/spawn_model)
  /head_camera/
    crop_decimate (nodelet/nodelet)
    head_camera_nodelet_manager (nodelet/nodelet)
  /head_camera/depth_downsample/
    points_downsample (nodelet/nodelet)

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

setting /run_id to 504a1e90-33d7-11eb-ac21-04ed33eed89c
process[rosout-1]: started with pid [3777]
started core service [/rosout]
process[gazebo-2]: started with pid [3784]
process[gazebo_gui-3]: started with pid [3789]
process[robot_state_publisher-4]: started with pid [3794]
process[urdf_spawner-5]: started with pid [3795]
process[prepare_robot-6]: started with pid [3800]
[ WARN] [WallTime: 1606828543.422825604] [node:/robot_state_publisher] [func:treeFromUrdfModel]: 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.
process[head_camera/head_camera_nodelet_manager-7]: started with pid [3802]
process[head_camera/crop_decimate-8]: started with pid [3804]
process[head_camera/depth_downsample/points_downsample-9]: started with pid [3807]
[ INFO] [WallTime: 1606828543.459413047] [node:/head_camera/crop_decimate] [func:loadNodelet]: Loading nodelet /head_camera/crop_decimate of type image_proc/crop_decimate to manager /head_camera/head_camera_nodelet_manager with the following remappings:
[ INFO] [WallTime: 1606828543.460719433] [node:/head_camera/crop_decimate] [func:loadNodelet]: /head_camera/camera/camera_info -> /head_camera/depth_registered/camera_info
[ INFO] [WallTime: 1606828543.460755334] [node:/head_camera/crop_decimate] [func:loadNodelet]: /head_camera/camera/image_raw -> /head_camera/depth_registered/image_raw
[ INFO] [WallTime: 1606828543.460774344] [node:/head_camera/crop_decimate] [func:loadNodelet]: /head_camera/camera_out -> /head_camera/depth_downsample
[ INFO] [WallTime: 1606828543.463059705] [node:/head_camera/crop_decimate] [func:service::exists]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] has not been advertised, waiting...
process[publish_base_scan_raw-10]: started with pid [3815]
[ INFO] [WallTime: 1606828543.487320064] [node:/head_camera/depth_downsample/points_downsample] [func:loadNodelet]: Loading nodelet /head_camera/depth_downsample/points_downsample of type depth_image_proc/point_cloud_xyz to manager /head_camera/head_camera_nodelet_manager with the following remappings:
process[cmd_vel_mux-11]: started with pid [3820]
[ INFO] [WallTime: 1606828543.492815842] [node:/head_camera/depth_downsample/points_downsample] [func:loadNodelet]: /head_camera/depth_downsample/image_rect -> /head_camera/depth_downsample/image_raw
[ INFO] [WallTime: 1606828543.494966318] [node:/head_camera/depth_downsample/points_downsample] [func:service::exists]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [WallTime: 1606828543.501866786] [node:/head_camera/head_camera_nodelet_manager] [func:Loader::Impl::advertiseRosApi]: Initializing nodelet with 2 worker threads.
[ INFO] [WallTime: 1606828543.506323517] [node:/head_camera/crop_decimate] [func:service::waitForService]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] is now available.
[ INFO] [WallTime: 1606828543.516495719] [node:/head_camera/depth_downsample/points_downsample] [func:service::waitForService]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] is now available.
[ INFO] [WallTime: 1606828543.781663982] [node:/gazebo] [func:GazeboRosApiPlugin::Load]: Finished loading Gazebo ROS API Plugin.
[ INFO] [WallTime: 1606828543.783198124] [node:/gazebo] [func:service::exists]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [WallTime: 1606828543.856049680] [node:/gazebo_gui] [func:GazeboRosApiPlugin::Load]: Finished loading Gazebo ROS API Plugin.
[ INFO] [WallTime: 1606828543.857064815] [node:/gazebo_gui] [func:service::exists]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[INFO] [WallTime: 1606828544.366230, 0.000000] [node:/urdf_spawner] [func:SpawnModelNode.run]: Loading model XML from ros parameter robot_description
[INFO] [WallTime: 1606828544.381295, 0.000000] [node:/urdf_spawner] [func:spawn_urdf_model_client]: Waiting for service /gazebo/spawn_urdf_model
Warning [parser.cc:626] Can not find the XML attribute 'version' in sdf XML tag for model: demo_cube. Please specify the SDF protocol supported in the model configuration file. The first sdf tag in the config file will be used 
[ INFO] [WallTime: 1606828544.497885538] [node:/gazebo] [func:service::waitForService]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [WallTime: 1606828544.514546962] [node:/gazebo] [func:GazeboRosApiPlugin::physicsReconfigureThread]: Physics dynamic reconfigure ready.
[INFO] [WallTime: 1606828544.684319, 0.142000] [node:/urdf_spawner] [func:spawn_urdf_model_client]: Calling service /gazebo/spawn_urdf_model
[ INFO] [WallTime: 1606828545.132726209, 0.333000000] [node:/gazebo] [func:__cxx11::string gazebo::GetRobotNamespace]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [WallTime: 1606828545.132784387, 0.333000000] [node:/gazebo] [func:GazeboRosLaser::Load]: Starting Laser Plugin (ns = /)
[ INFO] [WallTime: 1606828545.133576753, 0.333000000] [node:/gazebo] [func:GazeboRosLaser::LoadThread]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[INFO] [WallTime: 1606828545.191140, 0.333000] [node:/urdf_spawner] [func:spawn_urdf_model_client]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [WallTime: 1606828545.336729592, 0.333000000] [node:/gazebo] [func:__cxx11::string gazebo::GetRobotNamespace]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [WallTime: 1606828545.338805349, 0.333000000] [node:/gazebo] [func:GazeboRosCameraUtils::LoadThread]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[urdf_spawner-5] process has finished cleanly
log file: /home/sktometometo/.ros/log/504a1e90-33d7-11eb-ac21-04ed33eed89c/urdf_spawner-5*.log
[ WARN] [WallTime: 1606828545.830403748, 0.333000000] [node:/gazebo] [func:treeFromUrdfModel]: 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] [WallTime: 1606828545.832453198, 0.333000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started arm_controller/gravity_compensation
[ INFO] [WallTime: 1606828545.845240266, 0.333000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started base_controller
[ WARN] [WallTime: 1606828545.859168307, 0.333000000] [node:/gazebo] [func:treeFromUrdfModel]: 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] [WallTime: 1606828545.880689428, 0.333000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started bellows_controller
[ INFO] [WallTime: 1606828545.886735007, 0.333000000] [node:/gazebo] [func:FetchGazeboPlugin::Init]: Finished initializing FetchGazeboPlugin
[ INFO] [WallTime: 1606828546.476321140, 0.923000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started head_controller/follow_joint_trajectory
[ INFO] [WallTime: 1606828546.476447608, 0.923000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started arm_controller/follow_joint_trajectory
[ INFO] [WallTime: 1606828546.476572344, 0.923000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started gripper_controller/gripper_action
[prepare_robot-6] process has finished cleanly
log file: /home/sktometometo/.ros/log/504a1e90-33d7-11eb-ac21-04ed33eed89c/prepare_robot-6*.log
[ INFO] [WallTime: 1606828604.557254216, 51.594000000] [node:/gazebo] [func:ControllerManager::requestStart]: Stopped head_controller/follow_joint_trajectory
[ INFO] [WallTime: 1606828604.557355111, 51.594000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started head_controller/point_head
[ INFO] [WallTime: 1606829042.394880280, 367.428000000] [node:/gazebo] [func:ControllerManager::requestStart]: Started torso_controller/follow_joint_trajectory

~ $ roslaunch fetch_gazebo_demo demo.launch 
... logging to /home/sktometometo/.ros/log/504a1e90-33d7-11eb-ac21-04ed33eed89c/roslaunch-Persing-4373.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://Persing:45431/

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.005
 * /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/IKFastK...
 * /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.10

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 [4398]
process[amcl-2]: started with pid [4399]
process[move_base-3]: started with pid [4400]
process[tilt_head_node-4]: started with pid [4406]
process[move_group-5]: started with pid [4415]
process[basic_grasping_perception-6]: started with pid [4418]
process[demo-7]: started with pid [4420]
[ WARN] [WallTime: 1606828596.888583374] [node:/amcl] [func:requestMap]: Request for map failed; trying again...
[ INFO] [WallTime: 1606828596.930680090] [node:/move_group] [func:core::RobotModel::buildModel]: Loading robot model 'fetch'...
[ INFO] [WallTime: 1606828596.933259524] [node:/move_group] [func:core::JointModel* moveit::core::RobotModel::constructJointModel]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [WallTime: 1606828597.046173471, 46.300000000] [node:/move_base] [func:Costmap2DROS::Costmap2DROS]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 46.3 timeout was 0.1.
[ WARN] [WallTime: 1606828597.168020636, 46.386000000] [node:/move_group] [func:core::SolverAllocatorFn kinematics_plugin_loader::KinematicsPluginLoader::getLoaderFunction]: Kinematics solver doesn't support #attempts anymore, but only a timeout.
Please remove the parameter '/robot_description_kinematics/arm/kinematics_solver_attempts' from your configuration.
[ WARN] [WallTime: 1606828597.202335940, 46.413000000] [node:/move_group] [func:treeFromUrdfModel]: 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] [WallTime: 1606828597.530208236, 46.585000000] [node:/move_group] [func:PlanningSceneMonitor::startPublishingPlanningScene]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [WallTime: 1606828597.535487512, 46.589000000] [node:/move_group] [func:main]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [WallTime: 1606828597.535544537, 46.589000000] [node:/move_group] [func:PlanningSceneMonitor::startSceneMonitor]: Starting planning scene monitor
[ INFO] [WallTime: 1606828597.541462918, 46.590000000] [node:/move_group] [func:PlanningSceneMonitor::startSceneMonitor]: Listening to '/planning_scene'
[ INFO] [WallTime: 1606828597.541575870, 46.590000000] [node:/move_group] [func:PlanningSceneMonitor::startWorldGeometryMonitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [WallTime: 1606828597.544982982, 46.590000000] [node:/move_group] [func:PlanningSceneMonitor::startWorldGeometryMonitor]: Listening to '/collision_object'
[ INFO] [WallTime: 1606828597.547871406, 46.590000000] [node:/move_group] [func:PlanningSceneMonitor::startWorldGeometryMonitor]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [WallTime: 1606828597.548844323, 46.590000000] [node:/move_group] [func:OccupancyMapMonitor::initialize]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ERROR] [WallTime: 1606828597.550081573, 46.590000000] [node:/move_group] [func:OccupancyMapMonitor::initialize]: Failed to find 3D sensor plugin parameters for octomap generation
[ INFO] [WallTime: 1606828597.627750911, 46.657000000] [node:/move_group] [func:PlanningSceneMonitor::startStateMonitor]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [WallTime: 1606828597.672661763, 46.690000000] [node:/move_group] [func:OMPLInterface::OMPLInterface]: Initializing OMPL interface using ROS parameters
[ INFO] [WallTime: 1606828597.733526052, 46.727000000] [node:/move_group] [func:PlanningPipeline::configure]: Using planning interface 'OMPL'
[ INFO] [WallTime: 1606828597.740205284, 46.733000000] [node:/move_group] [func:FixWorkspaceBounds::FixWorkspaceBounds]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [WallTime: 1606828597.741003912, 46.734000000] [node:/move_group] [func:FixStartStateBounds::FixStartStateBounds]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [WallTime: 1606828597.741709966, 46.734000000] [node:/move_group] [func:FixStartStateBounds::FixStartStateBounds]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [WallTime: 1606828597.742516494, 46.735000000] [node:/move_group] [func:FixStartStateCollision::FixStartStateCollision]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [WallTime: 1606828597.743430325, 46.736000000] [node:/move_group] [func:FixStartStateCollision::FixStartStateCollision]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [WallTime: 1606828597.743974296, 46.736000000] [node:/move_group] [func:FixStartStateCollision::FixStartStateCollision]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [WallTime: 1606828597.744070517, 46.736000000] [node:/move_group] [func:PlanningPipeline::configure]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [WallTime: 1606828597.744114144, 46.736000000] [node:/move_group] [func:PlanningPipeline::configure]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [WallTime: 1606828597.744143679, 46.736000000] [node:/move_group] [func:PlanningPipeline::configure]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [WallTime: 1606828597.744198211, 46.736000000] [node:/move_group] [func:PlanningPipeline::configure]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [WallTime: 1606828597.744267045, 46.736000000] [node:/move_group] [func:PlanningPipeline::configure]: Using planning request adapter 'Fix Start State Path Constraints'
[ WARN] [WallTime: 1606828598.028218554, 46.935000000] [node:/move_base] [func:Costmap2DROS::checkOldParam]: global_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided
[ INFO] [WallTime: 1606828598.029963245, 46.936000000] [node:/move_base] [func:Costmap2DROS::Costmap2DROS]: global_costmap: Using plugin "static_map"
[ INFO] [WallTime: 1606828598.040967690, 46.944000000] [node:/move_base] [func:StaticLayer::onInitialize]: Requesting the map...
[ INFO] [WallTime: 1606828598.049378664, 46.952000000] [node:/move_group] [func:MoveItSimpleControllerManager::MoveItSimpleControllerManager]: Added FollowJointTrajectory controller for arm_controller
[ INFO] [WallTime: 1606828598.199046341, 47.049000000] [node:/move_base] [func:StaticLayer::incomingMap]: Resizing costmap to 401 X 444 at 0.050000 m/pix
[ INFO] [WallTime: 1606828598.352474572, 47.137000000] [node:/move_group] [func:MoveItSimpleControllerManager::MoveItSimpleControllerManager]: Added FollowJointTrajectory controller for arm_with_torso_controller
[ INFO] [WallTime: 1606828598.370376849, 47.149000000] [node:/move_base] [func:StaticLayer::onInitialize]: Received a 401 X 444 map at 0.050000 m/pix
[ INFO] [WallTime: 1606828598.375655296, 47.152000000] [node:/move_base] [func:Costmap2DROS::Costmap2DROS]: global_costmap: Using plugin "obstacles"
[ INFO] [WallTime: 1606828598.563646082, 47.258000000] [node:/move_group] [func:MoveItSimpleControllerManager::MoveItSimpleControllerManager]: Added GripperCommand controller for gripper_controller
[ INFO] [WallTime: 1606828598.563833105, 47.258000000] [node:/move_group] [func:MoveItSimpleControllerManager::getControllersList]: Returned 3 controllers in list
[ INFO] [WallTime: 1606828598.573310124, 47.265000000] [node:/move_base] [func:ObstacleLayer::onInitialize]:     Subscribed to Topics: base_scan
[ INFO] [WallTime: 1606828598.603163506, 47.285000000] [node:/move_group] [func:TrajectoryExecutionManager::initialize]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[ INFO] [WallTime: 1606828598.685161495, 47.331000000] [node:/move_base] [func:Costmap2DROS::Costmap2DROS]: global_costmap: Using plugin "inflater"
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] [WallTime: 1606828598.810317226, 47.416000000] [node:/move_group] [func:MoveGroupExe::configureCapabilities]: 

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

[ INFO] [WallTime: 1606828598.810472166, 47.416000000] [node:/move_group] [func:MoveGroupContext::status]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [WallTime: 1606828598.810531710, 47.416000000] [node:/move_group] [func:MoveGroupContext::status]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [WallTime: 1606828598.833878900, 47.432000000] [node:/move_base] [func:Costmap2DROS::Costmap2DROS]: local_costmap: Using plugin "obstacles"
[ INFO] [WallTime: 1606828598.840428639, 47.435000000] [node:/move_base] [func:ObstacleLayer::onInitialize]:     Subscribed to Topics: base_scan
[ INFO] [WallTime: 1606828598.917091866, 47.482000000] [node:/move_base] [func:Costmap2DROS::Costmap2DROS]: local_costmap: Using plugin "inflater"
[ INFO] [WallTime: 1606828598.990388534, 47.529000000] [node:/move_base] [func:MoveBase::MoveBase]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [WallTime: 1606828599.003441515, 47.539000000] [node:/move_base] [func:TrajectoryPlannerROS::initialize]: Sim period is set to 0.04
[ WARN] [WallTime: 1606828599.010334165, 47.544000000] [node:/move_base] [func:loadParameterWithDeprecation]: Parameter pdist_scale is deprecated. Please use the name path_distance_bias instead.
[ WARN] [WallTime: 1606828599.011897912, 47.545000000] [node:/move_base] [func:loadParameterWithDeprecation]: Parameter gdist_scale is deprecated. Please use the name goal_distance_bias instead.
[ INFO] [WallTime: 1606828599.550666759, 47.933000000] [node:/move_base] [func:ClearCostmapRecovery::initialize]: Recovery behavior will clear layer 'obstacles'
[ INFO] [WallTime: 1606828599.559889194, 47.940000000] [node:/move_base] [func:ClearCostmapRecovery::initialize]: Recovery behavior will clear layer 'obstacles'
[ INFO] [WallTime: 1606828599.621481991, 47.980000000] [node:/move_base] [func:OdometryHelperRos::odomCallback]: odom received!
[INFO] [WallTime: 1606828601.511732, 49.340000] [node:/demo] [func:MoveBaseClient.__init__]: Waiting for move_base...
[INFO] [WallTime: 1606828601.790860, 49.561000] [node:/demo] [func:FollowTrajectoryClient.__init__]: Waiting for torso_controller...
[INFO] [WallTime: 1606828602.093564, 49.766000] [node:/demo] [func:PointHeadClient.__init__]: Waiting for head_controller...
[INFO] [WallTime: 1606828602.382999, 49.982000] [node:/demo] [func:PlanningSceneInterface.__init__]: Waiting for get_planning_scene
[INFO] [WallTime: 1606828602.392725, 49.988000] [node:/demo] [func:PickPlaceInterface.__init__]: Connecting to pickup action...
[INFO] [WallTime: 1606828602.688209, 50.211000] [node:/demo] [func:PickPlaceInterface.__init__]: ...connected
[INFO] [WallTime: 1606828602.690381, 50.213000] [node:/demo] [func:PickPlaceInterface.__init__]: Connecting to place action...
[INFO] [WallTime: 1606828602.990166, 50.432000] [node:/demo] [func:PickPlaceInterface.__init__]: ...connected
[INFO] [WallTime: 1606828603.295561, 50.659000] [node:/demo] [func:GraspingClient.__init__]: Waiting for basic_grasping_perception/find_objects...
[INFO] [WallTime: 1606828603.589035, 50.876000] [node:/demo] [func:<module>]: Moving to table...
[ WARN] [WallTime: 1606828624.444793113, 66.638000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828637.976593027, 76.718000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828660.695720706, 93.149000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (0.50m).
[ERROR] [WallTime: 1606828674.225478740, 103.228000000] [node:/move_base] [func:MoveBase::executeCycle]: Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors
[ WARN] [WallTime: 1606828681.966613750, 109.073000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828689.030139727, 114.111000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828712.522178676, 130.503000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828725.050462616, 138.981000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828748.938910745, 155.271000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828756.841809105, 160.311000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828777.261143266, 174.261000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828784.667815867, 179.301000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828805.493947365, 195.181000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828818.451555211, 205.261000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828833.952878050, 216.651000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (0.50m).
[ WARN] [WallTime: 1606828848.432822172, 226.731000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828874.704644934, 245.611000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828895.816367368, 260.063000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828903.665284646, 265.101000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828927.045389512, 281.031000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828952.642950339, 299.871000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606828973.164265990, 315.961000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (3.00m).
[ WARN] [WallTime: 1606828986.144639118, 326.041000000] [node:/move_base] [func:RotateRecovery::runBehavior]: Rotate recovery behavior started.
[ WARN] [WallTime: 1606829007.400543745, 342.521000000] [node:/move_base] [func:ClearCostmapRecovery::runBehavior]: Clearing both costmaps to unstuck robot (0.50m).
[ERROR] [WallTime: 1606829042.358124593, 367.401000000] [node:/move_base] [func:MoveBase::executeCycle]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
[INFO] [WallTime: 1606829042.392132, 367.425000] [node:/demo] [func:<module>]: Raising torso...
[INFO] [WallTime: 1606829050.088133, 373.459000] [node:/demo] [func:<module>]: Picking object...
Traceback (most recent call last):
  File "/opt/ros/melodic/lib/fetch_gazebo_demo/demo.py", line 274, in <module>
    grasping_client.updateScene()
  File "/opt/ros/melodic/lib/fetch_gazebo_demo/demo.py", line 152, in updateScene
    wait = False)
TypeError: addSolidPrimitive() got an unexpected keyword argument 'wait'
[demo-7] process has died [pid 4420, exit code 1, cmd /opt/ros/melodic/lib/fetch_gazebo_demo/demo.py __name:=demo __log:=/home/sktometometo/.ros/log/504a1e90-33d7-11eb-ac21-04ed33eed89c/demo-7.log].
log file: /home/sktometometo/.ros/log/504a1e90-33d7-11eb-ac21-04ed33eed89c/demo-7*.log

Expected behavior
It is expected that fetch will achieve the demo without any error.

Screenshots
Please see above.

catkin workspace (please complete the following information):

• fetch_gazebo version: 0.9.2
• Ubuntu version: 18.04
• ROS version: melodic
• current catkin workspace information wstool info: no catkin workspace

Additional context
Add any other context about the problem here.

Bug: Regression in simulated Fetch robot (Melodic, Gazebo 9)

I think the simulated Fetch robot is not so good in Gazebo version 9, compared to the current performance in Gazebo 2.

New software: Ubuntu 18.04, ROS Melodic, Gazebo 9
Current software: Ubuntu 14.04, ROS Indigo, Gazebo 2.2.3

Issue description:
When moving the Fetch robot base in simulation, the base motion is very jerky. Like the robot is driving on carpet or with variable friction.
The motion is so jerky that it makes the upper body and arm shake.
Driving straight is impossible.

This issue means the base navigation is not good and it breaks the demo. See issue #30.

Steps to reproduce:

Load Fetch robot in an empty world:

roslaunch fetch_gazebo simulation.launch
roslaunch fetch_gazebo_demo fetch_nav.launch

Either, test forward motion using tele-op:
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

Or command the robot to x=3, y=3:
rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "map"}, pose: {position: {x: 3.0, y: 3.0, z: 0.0}, orientation: {w: 1.0}}}'

Results:

Forward straight-line motion:

Drive robot forward in ROS Indigo, Gazebo2:
Previously the motion is smooth and the robot drives almost in a straight-line, with a small variance due to friction.
https://youtu.be/2XgbXEiBYdE

Drive robot forward in ROS Melodic, Gazebo9:
The robot motion is jerky and unstable.
https://youtu.be/XXnWqwZ_bqU

Move base using Navigation stack:

Navigate to position (3,3) in ROS Indigo, Gazebo2:
The robot moves quickly and smoothly, with a nice velocity curve.
https://youtu.be/YIsQBLa9Aig

Navigate to position (3,3) in ROS Melodic, Gazebo9:
The robot motion is jerky and the speed changes. The robot misses the goal position and takes multiple attempts.
https://youtu.be/FyyGwdU6qRE

---

I also try to drive the robot forward in RDS cloud sim (ROS Melodic). The motion is jerky and the robot does not drive straight.
https://youtu.be/62bjO1FIRQo

[FetchIt!] Final list of objects and simulation layout

Issue description:
The object types have not been finalized (#81) and the simulation world is out of date.

List of objects:
Can Fetch Robotics provide the final information for these:
1. Kit

• Classroom caddy, set of 6 colors (https://www.amazon.com/gp/product/B074Q2Y7WR)
• Brand: YuzuKids
• Dimensions: 9.2 x 5.1 x 9.2 inches (not confirmed yet)
  .
  Need to compare with Romanoff brand (https://www.amazon.com/dp/B005IGNGGG)

2. Bin containing screws

• Wash basin, green. (https://www.amazon.com/dp/B00K2NZSFM)
• Brand: Carlisle
• Dimensions: 22 x 15.7 x 5.4 inches (not confirmed yet)

3. Bin containing gearbox parts

• Exact model from Amazon. ?
• LxWxH measurements. ?

It seems fair to use the exact object models for an industrial robot, and we want to make new 3D models.

Fixes to simulation:
Can someone at Fetch Robotics edit the simulation world?

• Missing wall on front of Schunk table. (#82)
• There's a part in the chuck of the Schunk. (#82)
• No kits are spawned in the Montreal2019 world. There should be 3 on the table. (#83)
• Wrong type of bin containing screws.
• Wrong type of bin containing gearbox parts.
• Wrong colors of parts. Previously parts would be different colors. Now Alex said all parts should be gray.
• Location on table to place finished kits? Another bin? AR Tag? Square shape of colored tape?
  .

how to make fetch execute a pre-defined trajectory in gazebo.

I don't have any hardware now, and I download this package. how to make fetch execute a pre-defined trajectory in gazebo? can anybody help me and tell me more details?

Release for Jade

We do not plan to support robots on Jade, however users may desire simulated robots on Jade (especially to get Gazebo 5).

Dependent repositories still to be released into Jade:

• robot_controllers (Pull Request)
• robot_calibration (so we can release fetch_ros repo) (Pull Request)
• warehouse_ros (so we can release moveit_ros)
• moveit_ros (maybe other packages in moveit)
• moveit_planners (ros/rosdistro#9073)
• fetch_ros (fetch_description, fetch_navigation, fetch_moveit_config)

Fixes to fetch_gazebo:

• update package.xml depends for gazebo 5
• one section of bellows and the estop are showing as white?

Possible improvements:

• use new friction models to improve simulation

Currently there is a jade-devel branch, but we may want to rename that "gazebo5" and then rename master to "gazebo2" as the gazebo version is actually the driving factor rather than the ROS version (for instance, you can use gazebo5 with indigo if you build fetch_gazebo and gazebo_ros from source as well)

[BUG] Arm gets into unusable position when using moveit and moving the torso

Describe the bug
The arm gets into an unusable state after controlling it with moveit and then moving the torso.
The arm seems to fall down and is able to go through the other parts of the robot. We can not control the arm after the arm gets into this state.

To Reproduce
Clone this repo: (https://github.com/GregoryLeMasurier/FetchArmTest)

Launch gazebo with fetch:
roslaunch fetchit_challenge main_arena_montreal2019_highlights.launch

Start move group:
roslaunch fetch_moveit_config move_group.launch

Move the arm:
rosrun test_ws arm_up_node

Move the torso:
rosrun test_ws torso_up_node

Try to move the arm while it is stuck in robot:
rosrun test_ws arm_up_node

Expected behavior
Arm doesn't move or fall after the torso moves.

Video
(https://youtu.be/yihiZyimFCI)

catkin workspace (please complete the following information):

• fetch_gazebo version: gazebo9
• Ubuntu version: 18.04
• ROS version: Melodic

[feature] Migrating to ROS2

Our research lab is in the progress of migrating our software stack to ROS2, and we like to start by testing out these infrastructure changes in simulation when doing so. I'd like to ask if there are any ongoing plans or current progress in updating the fetch ecosystem to use ROS2, be it in general for the hardware platform, or just for gazebo simulation, as we'd like to avoid duplicate efforts. We'd like to reach out to not only to Fetch, but any end user/developers to pool together any contributions.

We are also exploring the use of the ros2/ros1_bridge , but to me that's a short term solution.

Feature: Add option to start low-quality simulation for fetchit_challenge

Hello, great job with the new simulation. It looks very nice.

Issue description:
I start the simulation environment like this:
roslaunch fetchit_challenge main.launch
I wonder if all the extra objects make it slower to render or simulate on my computer.

Feature request:
Add a parameter to start a low-quality simulation.
This could disable spawning of objects like the carpet, pallet racks, barrels and factory walls.

gearbox_top and gearbox_largegear fall through the world

Related to #50 and #38

@RDaneelOlivav the simulation environment is looking good but there is still some fine tuning which needs to be done.

the large gear and gearbox top, fall through the world

[Fetchit!] [BUG] Rotated gears "stand up" over time

@RDaneelOlivav When I rotate or spawn in a gear on its side it will start to stand up as seen in the pictures below.
Being able to rotate a gear in simulation would be really useful for testing because in the competition some of the gears will be in this orientation.

To Reproduce
Change the roll, pitch, or yaw of any gear's pose to be 1.2.
Change the z of the gear to be above the schunk table (0.83)

Gear after rotation is applied:

Gear a few seconds after rotation is applied:

[pick_place_demo.py] cannot grasp the cube

Related to issue #30, I tried to run the pick_place_demo.py without the navigation part:

roslaunch fetch_gazebo pickplace_playground.launch
roslaunch fetch_gazebo_demo pick_place_demo.launch

It seems there are some error detecting the cube.

Grasping:

Picking:

Here is the full log:

$ roslaunch fetch_gazebo_demo pick_place_demo.launch
... logging to /home/zhao/.ros/log/e097f2ea-3066-11e9-8d6a-04d3b0082825/roslaunch-zhao1804-28988.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:42179/

SUMMARY

PARAMETERS

• /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_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
/
basic_grasping_perception (simple_grasping/basic_grasping_perception)
demo (fetch_gazebo_demo/pick_place_demo.py)
move_group (moveit_ros_move_group/move_group)

ROS_MASTER_URI=http://localhost:11311

running rosparam delete /move_group/sensors
ERROR: parameter [/move_group/sensors] is not set
process[move_group-1]: started with pid [29026]
process[basic_grasping_perception-2]: started with pid [29027]
process[demo-3]: started with pid [29028]
[ INFO] [1550155420.247534970]: Loading robot model 'fetch'...
[ INFO] [1550155420.249758237]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1550155420.697570069, 7.515000000]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ WARN] [1550155420.714784863, 7.516000000]: 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] [1550155421.051486137, 7.557000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1550155421.055649797, 7.558000000]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1550155421.055705277, 7.558000000]: Starting planning scene monitor
[ INFO] [1550155421.057952369, 7.559000000]: Listening to '/planning_scene'
[ INFO] [1550155421.057998972, 7.559000000]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1550155421.061246577, 7.560000000]: Listening to '/collision_object' using message notifier with target frame 'base_link '
[ INFO] [1550155421.065676134, 7.560000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1550155421.066743463, 7.560000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1550155421.174592184, 7.576000000]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1550155421.247784878, 7.585000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1550155421.316253356, 7.594000000]: Using planning interface 'OMPL'
[ INFO] [1550155421.326796233, 7.595000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1550155421.327774670, 7.595000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1550155421.328958543, 7.596000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550155421.332633334, 7.597000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550155421.333655127, 7.597000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1550155421.335969232, 7.597000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1550155421.336141642, 7.597000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1550155421.336213837, 7.597000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1550155421.336248030, 7.597000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1550155421.336663577, 7.597000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1550155421.336703310, 7.597000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1550155421.637563825, 7.681000000]: Added FollowJointTrajectory controller for arm_controller
[ INFO] [1550155421.927068015, 7.761000000]: Added FollowJointTrajectory controller for arm_with_torso_controller
[ INFO] [1550155422.260822953, 7.837000000]: Added GripperCommand controller for gripper_controller
[ INFO] [1550155422.261156981, 7.837000000]: Returned 3 controllers in list
[ INFO] [1550155422.343763647, 7.853000000]: 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] [1550155422.619851838, 7.897000000]:

---

• MoveGroup using:

• - ApplyPlanningSceneService
  

• - ClearOctomapService
  

• - CartesianPathService
  

• - ExecuteTrajectoryAction
  

• - GetPlanningSceneService
  

• - KinematicsService
  

• - MoveAction
  

• - PickPlaceAction
  

• - MotionPlanService
  

• - QueryPlannersService
  

• - StateValidationService
  

---

[ INFO] [1550155422.619959322, 7.897000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1550155422.619986510, 7.897000000]: MoveGroup context initialization complete

You can start planning now!

[INFO] [1550155425.182025, 8.641000]: Waiting for head_controller...
[INFO] [1550155425.332129, 8.685000]: Waiting for get_planning_scene
[INFO] [1550155425.380074, 8.699000]: Connecting to pickup action...
[INFO] [1550155425.683999, 8.812000]: ...connected
[INFO] [1550155425.685496, 8.812000]: Connecting to place action...
[INFO] [1550155425.991732, 8.929000]: ...connected
[INFO] [1550155426.291533, 9.044000]: Waiting for basic_grasping_perception/find_objects...
[ INFO] [1550155426.638234411, 9.176000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1550155426.638568602, 9.176000000]: Goal constraints are already satisfied. No need to plan or execute any motions
[INFO] [1550155429.294401, 10.189000]: Picking object...
[WARN] [1550155429.744109, 10.346000]: ObjectManager: object0 not added yet
[WARN] [1550155429.750793, 10.347000]: ObjectManager: surface0 not added yet
position:
x: 0.800304591656
y: 0.128784805536
z: 0.719509720802
orientation:
x: 0.00225892546587
y: -0.000417060917243
z: 0.99982714653
w: -0.0184499844909 type: 1
dimensions: [0.05848413705825806, 0.05982690304517746, 0.06015282869338989]
[INFO] [1550155430.282192, 10.547000]: Beginning to pick.
[ INFO] [1550155430.308622788, 10.555000000]: Planning attempt 1 of at most 1
[ INFO] [1550155430.335774344, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550155430.335932737, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550155430.335997754, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550155430.336049304, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550155430.336102672, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550155430.336161552, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550155430.336230476, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550155430.336293907, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550155430.336358307, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550155430.336417054, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550155430.336463869, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550155430.336524922, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550155430.336571700, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550155430.336630808, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550155430.336684226, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550155430.336746206, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550155430.337327785, 10.561000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550155430.337539936, 10.561000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550155430.347837217, 10.562000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155430.358707999, 10.568000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.359482408, 10.568000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155430.364262208, 10.572000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.370562400, 10.572000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155430.373513745, 10.575000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155430.383921179, 10.576000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.392562350, 10.583000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.398216377, 10.585000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.399175114, 10.586000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155430.420301382, 10.590000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.448059141, 10.600000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155431.431005965, 10.805000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155431.448205953, 10.806000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155431.456181845, 10.807000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155436.993896554, 11.798000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1550155436.999268899, 11.799000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1550155437.024884951, 11.805000000]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1550155437.027349463, 11.806000000]: Solution found in 0.028532 seconds
[ INFO] [1550155437.076598579, 11.814000000]: SimpleSetup: Path simplification took 0.047470 seconds and changed from 4 to 2 states
[ INFO] [1550155437.084802118, 11.814000000]: Found successful manipulation plan!
[ INFO] [1550155440.007407178, 12.296000000]: Manipulation plan 12 failed at stage 'approach & translate' on thread 0
[ INFO] [1550155440.018842795, 12.298000000]: Manipulation plan 13 failed at stage 'approach & translate' on thread 2
[ INFO] [1550155440.019213599, 12.298000000]: Pickup planning completed after 9.710337 seconds
[ WARN] [1550155463.913899004, 19.668000000]: Controller arm_controller failed with error code GOAL_TOLERANCE_VIOLATED
[ WARN] [1550155463.914225237, 19.668000000]: Controller handle arm_controller reports status ABORTED
[ INFO] [1550155463.914328516, 19.668000000]: Completed trajectory execution with status ABORTED ...
[ERROR] [1550155464.062538, 19.698000]: Pick failed during execution, attempting to cleanup.
[INFO] [1550155464.069791, 19.698000]: Pick did not grab object, try again...
[ INFO] [1550155464.102480738, 19.705000000]: Planning attempt 1 of at most 1
[ INFO] [1550155464.103529129, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550155464.103665656, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550155464.103727427, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550155464.103792350, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550155464.103840080, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550155464.103873440, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550155464.103908786, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550155464.103942663, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550155464.103976199, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550155464.104010031, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550155464.104041310, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550155464.104075503, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550155464.104110760, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550155464.104143595, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550155464.104175470, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550155464.104207791, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550155464.104238852, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550155464.104286326, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550155464.115045678, 19.705000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155464.121413457, 19.706000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155464.127622271, 19.706000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155464.134631177, 19.708000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155464.141600112, 19.708000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155464.148302253, 19.709000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155464.149319629, 19.709000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155464.155938859, 19.709000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155464.163443144, 19.709000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155464.180421518, 19.710000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155464.183416335, 19.710000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155464.189920666, 19.711000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155465.402118125, 19.831000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155465.412926059, 19.832000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155465.422259278, 19.834000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155471.756547482, 20.682000000]: Found a contact between 'object0' (type 'Object') and 'r_gripper_finger_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1550155471.756716465, 20.682000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1550155471.756875507, 20.682000000]: Start state appears to be in collision with respect to group arm
[ INFO] [1550155471.757233064, 20.682000000]: Found a valid state near the start state at distance 0.086531 after 0 attempts
[ INFO] [1550155471.758017345, 20.682000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1550155471.758566091, 20.682000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1550155471.773592136, 20.684000000]: RRTConnect: Created 5 states (3 start + 2 goal)
[ INFO] [1550155471.773683964, 20.684000000]: Solution found in 0.015258 seconds
[ INFO] [1550155471.824327334, 20.691000000]: SimpleSetup: Path simplification took 0.050551 seconds and changed from 4 to 2 states
[ INFO] [1550155471.830048520, 20.692000000]: Planning adapters have added states at index positions: [ 0 ]
[ INFO] [1550155471.837971940, 20.692000000]: Found successful manipulation plan!
[ INFO] [1550155473.663829588, 20.963000000]: Manipulation plan 13 failed at stage 'approach & translate' on thread 0
[ INFO] [1550155474.116754124, 21.039000000]: Manipulation plan 12 failed at stage 'approach & translate' on thread 3
[ INFO] [1550155474.116946183, 21.039000000]: Pickup planning completed after 10.014108 seconds
^C

[FetchIt!] Update the screw bin in simulation

@moriarty We saw last week the bin containing screws is updated. Could you update the simulation? @RDaneelOlivav Having the CAD file should be good for now, so we can add ourselves.

Sarah gives the Amazon link: https://www.amazon.com/dp/B00K2NZSFM/

Update 3D Parts

The 3D parts were originally sized before we were able to turn on the Schunk Machine.

It turned out, that the chuck was not able to close as much as we expected. We've made the shaft of the large gear larger to fit into the machine.

ZebraDevs/fetchit#2

@velveteenrobot

Mobile Manipulation Demo Experiences Several Trajectory Failures

I am trying to follow the tutorials on running the mobile robot demo.

http://docs.fetchrobotics.com/gazebo.html#running-the-mobile-manipulation-demo

However, the code doesn't seem to run the demo successfully where it is stated that:

demo.py - this our specific demo which navigates the robot from the starting pose in Gazebo to the table, raises the torso, lowers the head to look at the table, and then runs perception to generate a goal for MoveIt. The arm will then grasp the cube on the table, tuck the arm and lower the torso. Once the robot is back in this tucked configuration, the navigation stack will be once again called to navigate into the room with the countertop where the robot will place the cube on the other table.

First, a few preliminaries: I'm using Ubuntu 14.04 and Ros Indigo, and have installed the simulator:

>$ sudo apt-get update
>$ sudo apt-get install ros-indigo-fetch-gazebo-demo

Running the playground seems to work:

daniel@daniel-ubuntu-mac:/opt/ros/indigo/share/fetch_gazebo$ roslaunch fetch_gazebo playground.launch
... logging to /home/daniel/.ros/log/ca0ab696-4e23-11e8-bb5b-7831c1b89008/roslaunch-daniel-ubuntu-mac-4627.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://10.0.0.121:46069/

SUMMARY
========

PARAMETERS
 * /arm_controller/follow_joint_trajectory/joints: ['shoulder_pan_jo...

// skipping a lot of parameters here

 * /head_controller/point_head/type: robot_controllers...
 * /publish_base_scan_raw/lazy: True
 * /robot/serial: ABCDEFGHIJKLMNOPQ...
 * /robot/version: 0.0.1
 * /robot_description: <?xml version="1....
 * /robot_state_publisher/publish_frequency: 100.0
 * /rosdistro: indigo
 * /rosversion: 1.11.21
 * /torso_controller/follow_joint_trajectory/joints: ['torso_lift_joint']
 * /torso_controller/follow_joint_trajectory/type: robot_controllers...
 * /use_sim_time: True

NODES
  /head_camera/
    crop_decimate (nodelet/nodelet)
    head_camera_nodelet_manager (nodelet/nodelet)
  /
    cmd_vel_mux (topic_tools/mux)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    prepare_robot (fetch_gazebo/prepare_simulated_robot.py)
    publish_base_scan_raw (topic_tools/relay)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    urdf_spawner (gazebo_ros/spawn_model)
  /head_camera/depth_downsample/
    points_downsample (nodelet/nodelet)

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

setting /run_id to ca0ab696-4e23-11e8-bb5b-7831c1b89008
process[rosout-1]: started with pid [4655]
started core service [/rosout]
process[gazebo-2]: started with pid [4679]
process[gazebo_gui-3]: started with pid [4683]
process[robot_state_publisher-4]: started with pid [4687]
process[urdf_spawner-5]: started with pid [4688]
process[prepare_robot-6]: started with pid [4689]
process[head_camera/head_camera_nodelet_manager-7]: started with pid [4690]
process[head_camera/crop_decimate-8]: started with pid [4695]
process[head_camera/depth_downsample/points_downsample-9]: started with pid [4712]
process[publish_base_scan_raw-10]: started with pid [4723]
process[cmd_vel_mux-11]: started with pid [4731]
[ INFO] [1525277626.150857110]: Loading nodelet /head_camera/crop_decimate of type image_proc/crop_decimate to manager /head_camera/head_camera_nodelet_manager with the following remappings:
[ INFO] [1525277626.150908831]: /head_camera/camera/camera_info -> /head_camera/depth_registered/camera_info
[ INFO] [1525277626.150930140]: /head_camera/camera/image_raw -> /head_camera/depth_registered/image_raw
[ INFO] [1525277626.150946463]: /head_camera/camera_out -> /head_camera/depth_downsample
[ INFO] [1525277626.158022501]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1525277626.165092592]: Loading nodelet /head_camera/depth_downsample/points_downsample of type depth_image_proc/point_cloud_xyz to manager /head_camera/head_camera_nodelet_manager with the following remappings:
[ INFO] [1525277626.165162267]: /head_camera/depth_downsample/image_rect -> /head_camera/depth_downsample/image_raw
Gazebo multi-robot simulator, version 2.2.3
Copyright (C) 2012-2014 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[ INFO] [1525277626.170105554]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1525277626.176545210]: Initializing nodelet with 2 worker threads.
Gazebo multi-robot simulator, version 2.2.3
Copyright (C) 2012-2014 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Msg Waiting for master.[ INFO] [1525277626.273321984]: Finished loading Gazebo ROS API Plugin.
Msg Waiting for master[ INFO] [1525277626.273722966]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...

Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 10.0.0.121
spawn_model script started
[INFO] [WallTime: 1525277626.601051] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1525277626.604133] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1525277626.838549255, 0.023000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1525277626.867414146, 0.051000000]: Physics dynamic reconfigure ready.
[INFO] [WallTime: 1525277626.906568] [0.088000] Calling service /gazebo/spawn_urdf_model
[ INFO] [1525277627.021529520, 0.200000000]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] is now available.
[ INFO] [1525277627.022960629, 0.202000000]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] is now available.

Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 10.0.0.121
[ INFO] [1525277627.628650254, 0.270000000]: Laser Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[ INFO] [1525277627.628701855, 0.270000000]: Starting Laser Plugin (ns = /)!
[ INFO] [1525277627.630095150, 0.270000000]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1525277627.801777772, 0.270000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[INFO] [WallTime: 1525277627.803258] [0.270000] Spawn status: SpawnModel: Successfully spawned model
[ INFO] [1525277627.805694109, 0.270000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[urdf_spawner-5] process has finished cleanly
log file: /home/daniel/.ros/log/ca0ab696-4e23-11e8-bb5b-7831c1b89008/urdf_spawner-5*.log
[ WARN] [1525277629.347256986, 0.270000000]: 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] [1525277629.350078646, 0.270000000]: Started arm_controller/gravity_compensation
[ INFO] [1525277629.372639985, 0.270000000]: Started base_controller
[ WARN] [1525277629.391834470, 0.270000000]: 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] [1525277629.429908412, 0.270000000]: Started bellows_controller
[ INFO] [1525277629.439128851, 0.270000000]: Finished initializing FetchGazeboPlugin
[ INFO] [1525277630.020201435, 0.842000000]: Started gripper_controller/gripper_action
[ INFO] [1525277630.020572718, 0.843000000]: Started head_controller/follow_joint_trajectory
[ INFO] [1525277630.020712726, 0.843000000]: Started arm_controller/follow_joint_trajectory
[prepare_robot-6] process has finished cleanly
log file: /home/daniel/.ros/log/ca0ab696-4e23-11e8-bb5b-7831c1b89008/prepare_robot-6*.log

And I get the simulator up:

Now let's try and run the demo:

daniel@daniel-ubuntu-mac:/opt/ros/indigo/share/fetch_gazebo$ roslaunch fetch_gazebo_demo demo.launch
... logging to /home/daniel/.ros/log/ca0ab696-4e23-11e8-bb5b-7831c1b89008/roslaunch-daniel-ubuntu-mac-7012.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://10.0.0.121:42180/

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

// again skipping a bunch of parameters

 * /robot_description_planning/joint_limits/wrist_roll_joint/max_velocity: 2.26
 * /robot_description_semantic: <?xml version="1....
 * /rosdistro: indigo
 * /rosversion: 1.11.21

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

core service [/rosout] found
running rosparam delete /move_group/sensors
ERROR: parameter [/move_group/sensors] is not set
process[map_server-1]: started with pid [7035]
process[amcl-2]: started with pid [7036]
process[move_base-3]: started with pid [7037]
process[tilt_head_node-4]: started with pid [7047]
process[move_group-5]: started with pid [7061]
process[basic_grasping_perception-6]: started with pid [7064]
process[demo-7]: started with pid [7076]
[ INFO] [1525277912.983689711]: Loading robot model 'fetch'...
[ INFO] [1525277912.984216224]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1525277913.790123485, 234.841000000]: Loading robot model 'fetch'...
[ INFO] [1525277913.790427582, 234.841000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1525277913.967097893, 234.866000000]: 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] [1525277914.658743740, 235.022000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1525277914.673547181, 235.025000000]: MoveGroup debug mode is ON
Starting context monitors...
[ INFO] [1525277914.673708918, 235.025000000]: Starting scene monitor
[ INFO] [1525277914.688107967, 235.029000000]: Listening to '/planning_scene'
[ INFO] [1525277914.688240818, 235.029000000]: Starting world geometry monitor
[ INFO] [1525277914.702679628, 235.033000000]: Listening to '/collision_object' using message notifier with target frame '/base_link '
[ INFO] [1525277914.732044676, 235.041000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1525277914.736338564, 235.042000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[INFO] [WallTime: 1525277914.757083] [235.048000] Waiting for move_base...
[ INFO] [1525277914.963256078, 235.104000000]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1525277915.047228439, 235.124000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1525277915.202055702, 235.166000000]: Using planning interface 'OMPL'
[ INFO] [1525277915.217530911, 235.169000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1525277915.222677275, 235.170000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1525277915.224984609, 235.171000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1525277915.227074722, 235.171000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1525277915.229075479, 235.172000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1525277915.230882324, 235.172000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1525277915.231092324, 235.172000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1525277915.231155434, 235.172000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1525277915.231191858, 235.172000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1525277915.231270068, 235.172000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1525277915.231346605, 235.172000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ WARN] [1525277915.240566153, 235.175000000]: 
Deprecation warning: parameter 'allowed_execution_duration_scaling' moved into namespace 'trajectory_execution'.
Please, adjust file trajectory_execution.launch.xml!
[ WARN] [1525277915.242824760, 235.176000000]: 
Deprecation warning: parameter 'allowed_goal_duration_margin' moved into namespace 'trajectory_execution'.
Please, adjust file trajectory_execution.launch.xml!
[ INFO] [1525277915.545009302, 235.419000000]: Added FollowJointTrajectory controller for arm_controller
[ INFO] [1525277915.845527904, 235.703000000]: Added FollowJointTrajectory controller for arm_with_torso_controller
[ INFO] [1525277915.900202733, 235.753000000]: Using plugin "static_map"
[ INFO] [1525277915.906199098, 235.760000000]: Requesting the map...
[ INFO] [1525277916.152038547, 235.850000000]: Added GripperCommand controller for gripper_controller
[ INFO] [1525277916.152285531, 235.850000000]: Returned 3 controllers in list
[ INFO] [1525277916.202724540, 235.862000000]: Resizing costmap to 401 X 444 at 0.050000 m/pix
[ INFO] [1525277916.203667428, 235.862000000]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteService'...
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] [1525277916.533871687, 235.944000000]: 

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

[ INFO] [1525277916.534081020, 235.944000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1525277916.534154700, 235.944000000]: MoveGroup context initialization complete

All is well! Everyone is happy! You can start planning now!

[ INFO] [1525277916.600275123, 235.962000000]: Received a 401 X 444 map at 0.050000 m/pix
[ INFO] [1525277916.615331696, 235.967000000]: Using plugin "obstacles"
[ INFO] [1525277916.709042347, 235.990000000]:     Subscribed to Topics: base_scan
[ INFO] [1525277916.948503932, 236.053000000]: Using plugin "inflater"
[ INFO] [1525277917.244644546, 236.127000000]: Using plugin "obstacles"
[ INFO] [1525277917.256410995, 236.130000000]:     Subscribed to Topics: base_scan
[ INFO] [1525277917.464987323, 236.179000000]: Using plugin "inflater"
[ INFO] [1525277917.670913161, 236.229000000]: Created local_planner base_local_planner/TrajectoryPlannerROS
[ INFO] [1525277917.717531382, 236.241000000]: Sim period is set to 0.04
[ INFO] [1525277918.183530440, 236.625000000]: Recovery behavior will clear layer obstacles
[ INFO] [1525277918.191335098, 236.632000000]: Recovery behavior will clear layer obstacles
[ INFO] [1525277918.235636711, 236.673000000]: odom received!
[INFO] [WallTime: 1525277918.396332] [236.826000] Waiting for torso_controller...
[INFO] [WallTime: 1525277918.730244] [236.962000] Waiting for head_controller...
[INFO] [WallTime: 1525277919.003657] [237.034000] Waiting for get_planning_scene
[INFO] [WallTime: 1525277919.021412] [237.037000] Connecting to pickup action...
[INFO] [WallTime: 1525277919.315640] [237.118000] ...connected
[INFO] [WallTime: 1525277919.317862] [237.119000] Connecting to place action...
[INFO] [WallTime: 1525277919.596598] [237.192000] ...connected
[INFO] [WallTime: 1525277919.903001] [237.269000] Waiting for basic_grasping_perception/find_objects...
[INFO] [WallTime: 1525277920.193805] [237.346000] Moving to table...
[ WARN] [1525277967.656482459, 259.299000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2070 seconds
[ WARN] [1525277969.718895914, 260.137000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2450 seconds
[ WARN] [1525277974.242783754, 262.194000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1525277977.522313894, 263.714000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2220 seconds
[ WARN] [1525277978.663935013, 264.137000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.4450 seconds
[ WARN] [1525277985.065288953, 266.967000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2300 seconds
[ WARN] [1525277985.351015151, 267.234000000]: Rotate recovery behavior started.
[ WARN] [1525278001.182309837, 275.561000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2240 seconds
[ WARN] [1525278004.440285058, 276.786000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2490 seconds
[ WARN] [1525278005.014901480, 276.942000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2050 seconds
[ WARN] [1525278013.964425086, 280.248000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.3110 seconds
[ WARN] [1525278014.044180030, 280.275000000]: Clearing costmap to unstuck robot (0.500000m).
[ WARN] [1525278014.572083076, 280.435000000]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1525278021.092488338, 283.415000000]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2780 seconds
[INFO] [WallTime: 1525278026.350329] [285.763000] Raising torso...
[INFO] [WallTime: 1525278035.348610] [290.777000] Picking object...
[WARN] [WallTime: 1525278036.563935] [291.346000] Perception failed.
[INFO] [WallTime: 1525278036.564300] [291.346000] Picking object...
[WARN] [WallTime: 1525278037.096328] [291.808000] Perception failed.
[INFO] [WallTime: 1525278037.098032] [291.809000] Picking object...
[WARN] [WallTime: 1525278038.652184] [292.358000] Perception failed.
[INFO] [WallTime: 1525278038.652645] [292.358000] Picking object...
[WARN] [WallTime: 1525278039.528947] [293.053000] Perception failed.
[INFO] [WallTime: 1525278039.530537] [293.053000] Picking object...
[WARN] [WallTime: 1525278041.088222] [293.587000] Perception failed.
[INFO] [WallTime: 1525278041.088805] [293.587000] Picking object...

From the simulator, the first interesting thing is that the robot doesn't rotate correctly to start:

It was a bit stuck in this and rotating for a while, but as the debug messages above show, there is recovery behavior in this code. My machine is a bit old, so perhaps the slower the machine the more likely we run into these warning messages: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2070 seconds?

The real problem, however, is that once the Fetch rotates and raises its torso, the perception fails. From the simulator, it looks like the robot can't even see the cube.

I had to kill the program.

I ran this again but got a different issue; the robot was able to go to the second table --- but it didn't grasp the block, the image below was after the end (the block is out of view, sorry, but is still on the first table).

I guess there's two questions out of this:

• Is this kind of behavior expected for the simulator, even for a relatively fixed environment like this one?
• How do you debug the demo.py code? As in, how do you figure out the locations where to command the Fetch to, increase robustness, etc?

Thanks for any help you may provide! Happy to also provide more info as needed.

Quick update: I ran this a third time, this time using a better graphics card (not sure if that matters) and the trajectory was able to succeed, so the robot put the block at the second table. Interesting...

How to get Fetch and Gazebo9 installed and tested?

Hi Fetch Support,

We have an Ubuntu 18.04 machine with Ros Melodic and Gazebo9 installed:

adiganapathi@hermes1-ubuntu18:~/catkin_ws$ gazebo --version
Gazebo multi-robot simulator, version 9.0.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org


Gazebo multi-robot simulator, version 9.0.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org


adiganapathi@hermes1-ubuntu18:~/catkin_ws$ 

We are trying to get the Fetch robot started in the Gazebo simulator. We have read the other issues here and are aware that there are several problems with the simulator, but we would like to even get the simulator started so that we can observe these problems. The question is, how to install?

In our Ubuntu 14.04 machines with Fetch installed, running the Fetch on the Gazebo is easy because it just involves installing a package as shown in your README.

However, such a package does not appear to be available for Ubuntu 18.04, as suggested by this question on Ros Answers. That question was answered on October 2018, so perhaps there has been an update since then? But we cannot do a sudo apt-get installed. This is what we see after doing a sudo apt-get update beforehand:

$ sudo apt-get install ros-melodic-fetch-gazebo-demo
Reading package lists... Done
Building dependency tree       
Reading state information... Done
E: Unable to locate package ros-melodic-fetch-gazebo-demo

Next, we figured we would try a "manual" installation. To our knowledge, this is done by cloning this repository into catkin_ws/src and then doing catkin_make to build. Here is our output:

adiganapathi@hermes1-ubuntu18:~$ mkdir -p ~/catkin_ws/src
adiganapathi@hermes1-ubuntu18:~$ cd catkin_ws/src/
adiganapathi@hermes1-ubuntu18:~/catkin_ws/src$ git clone https://github.com/fetchrobotics/fetch_gazebo.git
Cloning into 'fetch_gazebo'...
remote: Enumerating objects: 34, done.
remote: Counting objects: 100% (34/34), done.
remote: Compressing objects: 100% (19/19), done.
remote: Total 1167 (delta 14), reused 33 (delta 14), pack-reused 1133
Receiving objects: 100% (1167/1167), 16.86 MiB | 31.00 MiB/s, done.
Resolving deltas: 100% (606/606), done.
adiganapathi@hermes1-ubuntu18:~/catkin_ws/src$ cd ..

And then running catkin_make results in the following output in the following pastebin.

It suggests to install some "robot_controllers" package, but we are not sure what that refers to, or if installing this is on the right track towards getting the Fetch displayed in the Gazebo.

Hopefully this issue shows what we are trying to do --- just getting the Fetch displayed in our Gazebo. Are these the right steps to follow? If not, where may we find the steps?

We're happy to provide any more information about our setup if necessary.

Thanks for your time!

[BUG] Fetch Robot doesn't tries to pick up the cube

I am launching the default simulation of picking up a cube from the table, built in fetch_gazebo package. The robot simply goes to the prepare position and doesn't move at all. (Initially I was working on a warehouse project with Fetch robot and was trying to perform SLAM with Laser and Odom but it was not working out, hence I started testing the default launch simulations.)

I installed fetch package using git clone. And I have all the packages installed as shown in the screenshot.

When I run roslaunch fetch_gazebo simple_grasp.launch , the model succesfully launches and comes to prepare pose and stops there. I get two warnings on the console as shown:

This is the setup:

I expect the robot to pick up the object successfully.

catkin workspace:

• fetch_gazebo version: Gazebo9
• Ubuntu version: 18.04
• ROS version:Melodic

[FetchIt!] Dockerfile & Docker Example

I was asked to share a Dockerfile with the example of catkin overlays. Sorry, I did have a small typo in the version which I sent around via email, the sed command had an extra ..

Here is how the robots for FetchIt! will be setup with three catkin layers:

1. /opt/ros/melodic <- bottom layer, only root user can install packages here
2. $HOME/ros/stable/install <- middle layer, contains packages which are not in ros-melodic-desktop-full but which you depend on, but which you don't actively develop on.
3. $HOME/ros/active/src <- your active catkin source space
   $HOME/ros/active/devel <- your active catkin devel space

FROM osrf/ros:melodic-desktop-full-bionic

RUN DEBIAN_FRONTEND=noninteractive apt-get update \
 && apt-get install -y -q --no-install-recommends \
    python-catkin-tools \
    python-rosinstall-generator \
 && . /opt/ros/melodic/setup.sh \
 && rosinstall_generator fetchit_challenge --deps --deps-only --exclude RPP > stable.rosinstall \
 && rosinstall_generator fetchit_challenge --upstream > active.rosinstall \
 && mkdir -p $HOME/ros/stable $HOME/ros/active \
 && wstool init $HOME/ros/stable/src stable.rosinstall \
 && wstool init $HOME/ros/active/src active.rosinstall \
 && cd $HOME/ros/stable \
    && catkin config --init \
    && catkin config --install --extend /opt/ros/melodic \
    && catkin build \
 && cd $HOME/ros/active \
    && catkin config --init \
    && catkin config --extend $HOME/ros/stable/install \
    && catkin build \
 && sed -i 's/\/opt\/ros\/\$ROS_DISTRO/\$HOME\/ros\/active\/devel/' ./ros_entrypoint.sh

ENTRYPOINT ["/ros_entrypoint.sh"]
CMD ["bash"]

gazebo9 branch

Melodic uses gazebo9 -- in the process of updating Navigation stack for Melodic, I had several patches for Gazebo API breaks.

If someone would kindly open a gazebo9 branch, I'll open a PR with the changes.

how to use arm_with_torso_controller/follow_joint_trajectory controller?

Thanks for your reply. And I try to use arm_with_torso_controller/follow_joint_trajectory controller to execute a pre-defined trajectory. It did not work. I can not find the reason, can you help me?

#!/usr/bin/env python


import sys

import rospy
import actionlib
from control_msgs.msg import (FollowJointTrajectoryAction,
                              FollowJointTrajectoryGoal)
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

arm_joint_names = ['l_wheel_joint', 'r_wheel_joint', 'torso_lift_joint', 'bellows_joint', 'head_pan_joint', 'head_tilt_joint','shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint',  'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint', 'l_gripper_finger_joint', 'r_gripper_finger_joint']
arm_joint_positions  =  [2.3345511882210843, -0.634820929568157, 1.3874957609389449e-11, 0.006625115661578738, -0.006317351703414076, 0.6313382345293581, 1.318234031118565, 1.3956276493932007, -0.2089051625781302, 1.7008202010543583, -0.016423456036472217, 1.6468142324488904, -0.008023511866540822, 0.02927438379458224, 0.02988700000824869]
velocity = [12.07760822395682, 12.531155056815715, 0.00551848394843501, -0.006297094941102944, -0.027727924188788666, -0.8389266373685278, -0.01006305278808928, 0.06190390257346417, 0.014009208799648233, 0.046633352775736804, -0.016161009170007727, 0.08276079812692123, -0.01060212434698555, -0.003579291965645062, 0.003610553717902847]
effort   = [8.85, 8.85, -450.0, -1.4943480513086147, 0.09047042943010608, -0.10530705118107508, -1.8543929008080047, 8.507774908689282, 1.273713991824661, 14.859113858920301, -7.935003515304931, -6.557514430844646, -1.7112672692034643, -9.663865221953854, -10.336134778046146]
if __name__ == "__main__":
    rospy.init_node("prepare_simulated_robot")


    if rospy.get_param("robot/serial") != "ABCDEFGHIJKLMNOPQRSTUVWX":
        rospy.logerr("This script should not be run on a real robot")
        sys.exit(-1)



    rospy.loginfo("Waiting for arm_controller...")
    arm_client = actionlib.SimpleActionClient("arm_with_torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
    arm_client.wait_for_server()
    rospy.loginfo("...connected.")

    trajectory = JointTrajectory()
    trajectory.joint_names = arm_joint_names

    trajectory.points.append(JointTrajectoryPoint())
    trajectory.points[0].positions = arm_joint_positions
    trajectory.points[0].velocities =  velocity
    trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions)
    trajectory.points[0].effort = effort
    trajectory.points[0].time_from_start = rospy.Duration(1.0)



    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(0.0)


    rospy.loginfo("Setting positions...")

    arm_client.send_goal(arm_goal)

    arm_client.wait_for_result(rospy.Duration(6.0))

    rospy.loginfo("...done")

catkin_make_isolated --install fails

http://build.ros.org/view/Idev/job/Idev__fetch_gazebo__ubuntu_trusty_amd64/9/

The build farm has gotten a lot smarter since this package was last released, our header files are not being installed.

I'll submit a PR to gazabo9 , gazebo2 and gazebo7 in a minute.

Gazebo9 Issues :-(

We have a problem with the Gazebo Robot Model. Things work fine on the Robot, but we've had a number of bugs reported with the simulated robot.

Update (March 20)

I've just added a help wanted label to this issue, as I've been tuning the model, and have attached the git diff to this issue, but we'll accept any PRs if anyone as time to try and further tune the models. It might be best if we work in parallel and communicate back via this issue.

---

We've been investigating the issue, and tuning/tweaking parameters. However we haven't found a solution yet. Thank you for the detailed bug reports, and youtube videos of gazebo.

#36 #35 #30 #31

I suspect, that Gazebo has gotten better, and we were relaying on some values which didn't make sense but use to work. Here is one inline comment which leads to that suspicion...

https://github.com/fetchrobotics/fetch_gazebo/blob/643217ce74c4c7537c2832a634944b4a528173c1/fetch_gazebo/robots/fetch.gazebo.xacro#L47

But in the past between gazebo jumps I've seen similar cases were we've had to go back and tune the kp kd values as gazebo gets better they needed to be updated.
http://gazebosim.org/tutorials/?tut=ros_urdf

Simulated IMU

I was testing out this package with gazebo and noticed that there was no imu messages being publish like for the real platform: http://docs.fetchrobotics.com/robot_hardware.html#imu

It'd be nice in general to add the IMUs in the gazebo models so I could test out projects like cartographer that use senors fusion with IMUs in simulation. I suppose I could do a PR, but I'm not sure of the IMUs' origin wrt the robot's baselink and arm link.

Bug: Simulation fetchit_challenge looks different in RDS & Gazebo

Cloud simulator: RDS (ROS Development Studio), Ubuntu 18.04, ROS Melodic
Local computer: Ubuntu 18.04, ROS Melodic, Gazebo 9, NVidia GPU

Has anyone compared the FetchIt Challenge simulator running on RDS with installing on local computer?

Bug description:
I find there are various differences, like objects that are not spawned or maybe they are just hidden.

• The two bolts are blue on RDS but orange on local computer.
• The two gears. On RDS there is a red and a green gear. My local computer is missing the large green gear.
• The 3 tables are transparent on local computer.
• The Schunk machine has different appearance on local computer, like half of it is white instead of grey.
• The lights are rendered differently. RDS looks more like Gazebo2. Running Gazebo9 on local computer, objects are either too bright or too dark.
• Non-essential objects like some barrels and a box are missing on local computer.

Here are some screenshots:

[FetchIt!] SCHUNK machine model shoudn't have the cylinder inserted

@moriarty Our team just know that we need to insert the gear inside the whole of the SCHUNK machine. In simulation, there is a half yellow half black cylinder inserted and should be removed.

@RDaneelOlivav We tried to use Blender to remove the cylindar from shunk_machine_lathe_centered.dae but the main.launch file won't launch probably because the pose also needs to be updated?

In sim:

In the arena:

Fetch navigation performs poorly in Melodic simulation

Steps
With up to date versions of fetch_ros and fetch_gazebo

roslaunch fetch_gazebo playground.launch

And

roslaunch fetch_gazeo_demo fetch_nav.launch 

Behavior
When given a nav goal, the robot's localization drifts quickly (seems like it happens during rotation). The robot is never able to reach the goal.

https://youtu.be/w1y0b5aI3o8

Nothing jumps out from the standard move_base configurations so I'm not sure what's going on.

controllers are not started in fetch_gazebo

Hi,

When launching roslaunch fetch_gazebo playground.launch a number of services are not
started and therefore the preparation script hangs as well. The result is a non-functioning simulated robot (base and arm).

I think this might be a hiccup because I run fetch_gazebo on kinetic. But also, the issue does not occur 100% of the time but maybe 90%.

This is the log:

output of roslaunch fetch_gazebo playground.launch

roslaunch fetch_gazebo playground.launch ... logging to /home/pholthaus/.ros/log/e8326c58-e7e9-11e8-9a5a-9cb6d0e2076f/roslaunch-rh-laptop-28376.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.

xacro: Traditional processing is deprecated. Switch to --inorder processing!
To check for compatibility of your document, use option --check-order.
For more infos, see http://wiki.ros.org/xacro#Processing_Order
xacro.py is deprecated; please use xacro instead
started roslaunch server http://192.168.1.66:46687/

SUMMARY

PARAMETERS

• /arm_controller/follow_joint_trajectory/joints: ['shoulder_pan_jo...
• /arm_controller/follow_joint_trajectory/type: robot_controllers...
• /arm_controller/gravity_compensation/autostart: True
• /arm_controller/gravity_compensation/root: torso_lift_link
• /arm_controller/gravity_compensation/tip: gripper_link
• /arm_controller/gravity_compensation/type: robot_controllers...
• /arm_with_torso_controller/follow_joint_trajectory/joints: ['torso_lift_join...
• /arm_with_torso_controller/follow_joint_trajectory/type: robot_controllers...
• /base_controller/autostart: True
• /base_controller/type: robot_controllers...
• /bellows_controller/autostart: True
• /bellows_controller/controlled_joint: bellows_joint
• /bellows_controller/mimic_joint: torso_lift_joint
• /bellows_controller/mimic_scale: 0.5
• /bellows_controller/type: robot_controllers...
• /gazebo/bellows_joint/position/d: 0.0
• /gazebo/bellows_joint/position/i: 0.0
• /gazebo/bellows_joint/position/i_clamp: 0.0
• /gazebo/bellows_joint/position/p: 10.0
• /gazebo/bellows_joint/velocity/d: 0.0
• /gazebo/bellows_joint/velocity/i: 0.0
• /gazebo/bellows_joint/velocity/i_clamp: 0.0
• /gazebo/bellows_joint/velocity/p: 25.0
• /gazebo/default_controllers: ['arm_controller/...
• /gazebo/elbow_flex_joint/position/d: 0.1
• /gazebo/elbow_flex_joint/position/i: 0.0
• /gazebo/elbow_flex_joint/position/i_clamp: 0.0
• /gazebo/elbow_flex_joint/position/p: 100.0
• /gazebo/elbow_flex_joint/velocity/d: 0.0
• /gazebo/elbow_flex_joint/velocity/i: 0.0
• /gazebo/elbow_flex_joint/velocity/i_clamp: 0.0
• /gazebo/elbow_flex_joint/velocity/p: 150.0
• /gazebo/forearm_roll_joint/position/d: 0.1
• /gazebo/forearm_roll_joint/position/i: 0.0
• /gazebo/forearm_roll_joint/position/i_clamp: 0.0
• /gazebo/forearm_roll_joint/position/p: 100.0
• /gazebo/forearm_roll_joint/velocity/d: 0.0
• /gazebo/forearm_roll_joint/velocity/i: 0.0
• /gazebo/forearm_roll_joint/velocity/i_clamp: 0.0
• /gazebo/forearm_roll_joint/velocity/p: 150.0
• /gazebo/head_pan_joint/position/d: 0.0
• /gazebo/head_pan_joint/position/i: 0.0
• /gazebo/head_pan_joint/position/i_clamp: 0.0
• /gazebo/head_pan_joint/position/p: 2.0
• /gazebo/head_pan_joint/velocity/d: 0.0
• /gazebo/head_pan_joint/velocity/i: 0.0
• /gazebo/head_pan_joint/velocity/i_clamp: 0.0
• /gazebo/head_pan_joint/velocity/p: 2.0
• /gazebo/head_tilt_joint/position/d: 0.0
• /gazebo/head_tilt_joint/position/i: 0.0
• /gazebo/head_tilt_joint/position/i_clamp: 0.0
• /gazebo/head_tilt_joint/position/p: 10.0
• /gazebo/head_tilt_joint/velocity/d: 0.0
• /gazebo/head_tilt_joint/velocity/i: 0.0
• /gazebo/head_tilt_joint/velocity/i_clamp: 0.0
• /gazebo/head_tilt_joint/velocity/p: 3.0
• /gazebo/l_gripper_finger_joint/position/d: 0.0
• /gazebo/l_gripper_finger_joint/position/i: 0.0
• /gazebo/l_gripper_finger_joint/position/i_clamp: 0.0
• /gazebo/l_gripper_finger_joint/position/p: 5000.0
• /gazebo/l_gripper_finger_joint/velocity/d: 0.0
• /gazebo/l_gripper_finger_joint/velocity/i: 0.0
• /gazebo/l_gripper_finger_joint/velocity/i_clamp: 0.0
• /gazebo/l_gripper_finger_joint/velocity/p: 0.0
• /gazebo/l_wheel_joint/position/d: 0.0
• /gazebo/l_wheel_joint/position/i: 0.0
• /gazebo/l_wheel_joint/position/i_clamp: 0.0
• /gazebo/l_wheel_joint/position/p: 0.0
• /gazebo/l_wheel_joint/velocity/d: 0.0
• /gazebo/l_wheel_joint/velocity/i: 0.5
• /gazebo/l_wheel_joint/velocity/i_clamp: 6.0
• /gazebo/l_wheel_joint/velocity/p: 8.85
• /gazebo/r_gripper_finger_joint/position/d: 0.0
• /gazebo/r_gripper_finger_joint/position/i: 0.0
• /gazebo/r_gripper_finger_joint/position/i_clamp: 0.0
• /gazebo/r_gripper_finger_joint/position/p: 5000.0
• /gazebo/r_gripper_finger_joint/velocity/d: 0.0
• /gazebo/r_gripper_finger_joint/velocity/i: 0.0
• /gazebo/r_gripper_finger_joint/velocity/i_clamp: 0.0
• /gazebo/r_gripper_finger_joint/velocity/p: 0.0
• /gazebo/r_wheel_joint/position/d: 0.0
• /gazebo/r_wheel_joint/position/i: 0.0
• /gazebo/r_wheel_joint/position/i_clamp: 0.0
• /gazebo/r_wheel_joint/position/p: 0.0
• /gazebo/r_wheel_joint/velocity/d: 0.0
• /gazebo/r_wheel_joint/velocity/i: 0.5
• /gazebo/r_wheel_joint/velocity/i_clamp: 6.0
• /gazebo/r_wheel_joint/velocity/p: 8.85
• /gazebo/shoulder_lift_joint/position/d: 0.1
• /gazebo/shoulder_lift_joint/position/i: 0.0
• /gazebo/shoulder_lift_joint/position/i_clamp: 0.0
• /gazebo/shoulder_lift_joint/position/p: 100.0
• /gazebo/shoulder_lift_joint/velocity/d: 0.0
• /gazebo/shoulder_lift_joint/velocity/i: 0.0
• /gazebo/shoulder_lift_joint/velocity/i_clamp: 0.0
• /gazebo/shoulder_lift_joint/velocity/p: 200.0
• /gazebo/shoulder_pan_joint/position/d: 0.1
• /gazebo/shoulder_pan_joint/position/i: 0.0
• /gazebo/shoulder_pan_joint/position/i_clamp: 0.0
• /gazebo/shoulder_pan_joint/position/p: 100.0
• /gazebo/shoulder_pan_joint/velocity/d: 0.0
• /gazebo/shoulder_pan_joint/velocity/i: 2.0
• /gazebo/shoulder_pan_joint/velocity/i_clamp: 1.0
• /gazebo/shoulder_pan_joint/velocity/p: 200.0
• /gazebo/torso_lift_joint/position/d: 0.0
• /gazebo/torso_lift_joint/position/i: 0.0
• /gazebo/torso_lift_joint/position/i_clamp: 0.0
• /gazebo/torso_lift_joint/position/p: 1000.0
• /gazebo/torso_lift_joint/velocity/d: 0.0
• /gazebo/torso_lift_joint/velocity/i: 0.0
• /gazebo/torso_lift_joint/velocity/i_clamp: 0.0
• /gazebo/torso_lift_joint/velocity/p: 100000.0
• /gazebo/upperarm_roll_joint/position/d: 0.1
• /gazebo/upperarm_roll_joint/position/i: 0.0
• /gazebo/upperarm_roll_joint/position/i_clamp: 0.0
• /gazebo/upperarm_roll_joint/position/p: 100.0
• /gazebo/upperarm_roll_joint/velocity/d: 0.0
• /gazebo/upperarm_roll_joint/velocity/i: 0.0
• /gazebo/upperarm_roll_joint/velocity/i_clamp: 0.0
• /gazebo/upperarm_roll_joint/velocity/p: 10.0
• /gazebo/wrist_flex_joint/position/d: 0.1
• /gazebo/wrist_flex_joint/position/i: 0.0
• /gazebo/wrist_flex_joint/position/i_clamp: 0.0
• /gazebo/wrist_flex_joint/position/p: 100.0
• /gazebo/wrist_flex_joint/velocity/d: 0.0
• /gazebo/wrist_flex_joint/velocity/i: 0.0
• /gazebo/wrist_flex_joint/velocity/i_clamp: 0.0
• /gazebo/wrist_flex_joint/velocity/p: 100.0
• /gazebo/wrist_roll_joint/position/d: 0.1
• /gazebo/wrist_roll_joint/position/i: 0.0
• /gazebo/wrist_roll_joint/position/i_clamp: 0.0
• /gazebo/wrist_roll_joint/position/p: 100.0
• /gazebo/wrist_roll_joint/velocity/d: 0.0
• /gazebo/wrist_roll_joint/velocity/i: 0.0
• /gazebo/wrist_roll_joint/velocity/i_clamp: 0.0
• /gazebo/wrist_roll_joint/velocity/p: 100.0
• /gripper_controller/gripper_action/centering/d: 0.0
• /gripper_controller/gripper_action/centering/i: 0.0
• /gripper_controller/gripper_action/centering/i_clamp: 0.0
• /gripper_controller/gripper_action/centering/p: 1000.0
• /gripper_controller/gripper_action/type: robot_controllers...
• /head_camera/crop_decimate/decimation_x: 4
• /head_camera/crop_decimate/decimation_y: 4
• /head_camera/crop_decimate/queue_size: 1
• /head_camera/head_camera_nodelet_manager/num_worker_threads: 2
• /head_controller/follow_joint_trajectory/joints: ['head_pan_joint'...
• /head_controller/follow_joint_trajectory/type: robot_controllers...
• /head_controller/point_head/type: robot_controllers...
• /publish_base_scan_raw/lazy: True
• /robot/serial: ABCDEFGHIJKLMNOPQ...
• /robot/version: 0.0.1
• /robot_description: <?xml version="1....
• /robot_state_publisher/publish_frequency: 100.0
• /rosdistro: kinetic
• /rosversion: 1.12.14
• /torso_controller/follow_joint_trajectory/joints: ['torso_lift_joint']
• /torso_controller/follow_joint_trajectory/type: robot_controllers...
• /use_sim_time: True

NODES
/head_camera/
crop_decimate (nodelet/nodelet)
head_camera_nodelet_manager (nodelet/nodelet)
/
cmd_vel_mux (topic_tools/mux)
gazebo (gazebo_ros/gzserver)
gazebo_gui (gazebo_ros/gzclient)
prepare_robot (fetch_gazebo/prepare_simulated_robot.py)
publish_base_scan_raw (topic_tools/relay)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
urdf_spawner (gazebo_ros/spawn_model)
/head_camera/depth_downsample/
points_downsample (nodelet/nodelet)

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

setting /run_id to e8326c58-e7e9-11e8-9a5a-9cb6d0e2076f
process[rosout-1]: started with pid [28408]
started core service [/rosout]
process[gazebo-2]: started with pid [28432]
process[gazebo_gui-3]: started with pid [28435]
process[robot_state_publisher-4]: started with pid [28442]
process[urdf_spawner-5]: started with pid [28443]
process[prepare_robot-6]: started with pid [28444]
process[head_camera/head_camera_nodelet_manager-7]: started with pid [28446]
process[head_camera/crop_decimate-8]: started with pid [28459]
process[head_camera/depth_downsample/points_downsample-9]: started with pid [28465]
process[publish_base_scan_raw-10]: started with pid [28476]
process[cmd_vel_mux-11]: started with pid [28498]
[ INFO] [1542185246.873225998]: Loading nodelet /head_camera/crop_decimate of type image_proc/crop_decimate to manager
[ INFO] [1542185246.873289624]: /head_camera/camera/camera_info -> /head_camera/depth_registered/camera_info
[ INFO] [1542185246.873318806]: /head_camera/camera/image_raw -> /head_camera/depth_registered/image_raw
[ INFO] [1542185246.873330187]: /head_camera/camera_out -> /head_camera/depth_downsample
[ INFO] [1542185246.882549974]: Loading nodelet /head_camera/depth_downsample/points_downsample of type depth_image_pro
[ INFO] [1542185246.882616359]: /head_camera/depth_downsample/image_rect -> /head_camera/depth_downsample/image_raw
[ INFO] [1542185246.884142151]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] has not
[ INFO] [1542185246.891591140]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodelet] has not
[ INFO] [1542185246.901519022]: Initializing nodelet with 2 worker threads.
[ INFO] [1542185247.418012625]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1542185247.419667573]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiti
[ INFO] [1542185247.453557713]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1542185247.455752611]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiti
SpawnModel script started
[INFO] [1542185247.753859, 0.000000]: Loading model XML from ros parameter
[INFO] [1542185247.764684, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1542185249.262816920, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1542185249.292099526, 0.050000000]: Physics dynamic reconfigure ready.
[INFO] [1542185249.295019, 0.053000]: Calling service /gazebo/spawn_urdf_model
[ INFO] [1542185249.719330735, 0.089000000]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1542185249.719524264, 0.089000000]: Starting Laser Plugin (ns = /)
[ INFO] [1542185249.723364851, 0.089000000]: Laser Plugin (ns = /) <tf_prefix_>, set to ""
[INFO] [1542185249.756532, 0.089000]: Spawn status: SpawnModel: Successfully spawned entity
[urdf_spawner-5] process has finished cleanly
log file: /home/pholthaus/.ros/log/e8326c58-e7e9-11e8-9a5a-9cb6d0e2076f/urdf_spawner-5*.log
[ INFO] [1542185250.272429846, 0.089000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1542185250.280935300, 0.089000000]: Camera Plugin (ns = /) <tf_prefix_>, set to ""
[ INFO] [1542185251.629368713, 0.112000000]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodlet] is now available.
[ INFO] [1542185251.632567338, 0.113000000]: waitForService: Service [/head_camera/head_camera_nodelet_manager/load_nodlet] is now available.
[ INFO] [1542185251.635794372, 0.117000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1542185251.879119571, 0.227000000]: Physics dynamic reconfigure ready.

output of roslaunch fetch_gazebo_demo demo.launch

... logging to /home/pholthaus/.ros/log/e8326c58-e7e9-11e8-9a5a-9cb6d0e2076f/roslaunch-rh-laptop-31109.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://192.168.1.66:41369/

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: kinetic
• /rosversion: 1.12.14

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 [31131]
process[amcl-2]: started with pid [31132]
process[move_base-3]: started with pid [31133]
process[tilt_head_node-4]: started with pid [31145]
process[move_group-5]: started with pid [31154]
process[basic_grasping_perception-6]: started with pid [31160]
process[demo-7]: started with pid [31166]
[ INFO] [1542185523.791829418]: Loading robot model 'fetch'...
[ INFO] [1542185523.791964673]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1542185524.338341153, 189.126000000]: Loading robot model 'fetch'...
[ INFO] [1542185524.338648278, 189.126000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1542185524.402074192, 189.132000000]: 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] [1542185524.678010313, 189.256000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1542185524.686233908, 189.260000000]: MoveGroup debug mode is ON
Starting context monitors...
[ INFO] [1542185524.686343429, 189.260000000]: Starting scene monitor
[ INFO] [1542185524.696080548, 189.264000000]: Listening to '/planning_scene'
[ INFO] [1542185524.696196675, 189.264000000]: Starting world geometry monitor
[ INFO] [1542185524.703605472, 189.267000000]: Listening to '/collision_object' using message notifier with target frame '/base_link '
[ INFO] [1542185524.710783894, 189.268000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1542185524.712376935, 189.269000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1542185524.778712790, 189.290000000]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1542185524.821031213, 189.302000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1542185524.891946078, 189.320000000]: Using planning interface 'OMPL'
[ INFO] [1542185524.898984352, 189.322000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1542185524.900026789, 189.322000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1542185524.900926647, 189.322000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1542185524.902259693, 189.323000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1542185524.903220965, 189.323000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1542185524.904232731, 189.323000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1542185524.904402453, 189.323000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1542185524.904499732, 189.323000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1542185524.904603734, 189.323000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1542185524.904692418, 189.323000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1542185524.904806756, 189.323000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ WARN] [1542185524.913389818, 189.325000000]:
Deprecation warning: parameter 'allowed_execution_duration_scaling' moved into namespace 'trajectory_execution'.
Please, adjust file trajectory_execution.launch.xml!
[ WARN] [1542185524.914700626, 189.325000000]:
Deprecation warning: parameter 'allowed_goal_duration_margin' moved into namespace 'trajectory_execution'.
Please, adjust file trajectory_execution.launch.xml!
[INFO] [1542185525.428820, 189.502000]: Waiting for move_base...
[ WARN] [1542185533.373046105, 194.022000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ WARN] [1542185533.998356608, 194.343000000]: Waiting for arm_controller/follow_joint_trajectory to come up
[ WARN] [1542185542.269917176, 199.122000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ WARN] [1542185544.304806759, 200.343000000]: Waiting for arm_controller/follow_joint_trajectory to come up
[ WARN] [1542185550.995992914, 204.123000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ERROR] [1542185554.752360977, 206.343000000]: Action client not connected: arm_controller/follow_joint_trajectory
[ WARN] [1542185559.631538364, 209.128000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ WARN] [1542185563.625625070, 211.372000000]: Waiting for arm_with_torso_controller/follow_joint_trajectory to come up
[ WARN] [1542185569.362045700, 214.137000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ WARN] [1542185575.015365092, 217.373000000]: Waiting for arm_with_torso_controller/follow_joint_trajectory to come up
[ WARN] [1542185577.873390713, 219.148000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ERROR] [1542185585.752152775, 223.373000000]: Action client not connected: arm_with_torso_controller/follow_joint_trajectory
[ WARN] [1542185587.439195139, 224.156000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ WARN] [1542185595.262815289, 228.408000000]: Waiting for gripper_controller/gripper_action to come up
[ WARN] [1542185596.709020536, 229.158000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ WARN] [1542185605.703078445, 234.168000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ WARN] [1542185606.201083150, 234.410000000]: Waiting for gripper_controller/gripper_action to come up
[ WARN] [1542185614.404625046, 239.171000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.1 timeout was 0.1.
[ERROR] [1542185616.457812125, 240.410000000]: Action client not connected: gripper_controller/gripper_action
[ INFO] [1542185616.466754953, 240.417000000]: Returned 0 controllers in list
[ INFO] [1542185616.489385262, 240.431000000]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteService'...
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] [1542185616.629419763, 240.527000000]:

---

• MoveGroup using:

• - ApplyPlanningSceneService
  

• - ClearOctomapService
  

• - CartesianPathService
  

• - ExecuteTrajectoryService
  

• - ExecuteTrajectoryAction
  

• - GetPlanningSceneService
  

• - KinematicsService
  

• - MoveAction
  

• - PickPlaceAction
  

• - MotionPlanService
  

• - QueryPlannersService
  

• - StateValidationService
  

---

[ INFO] [1542185616.629518238, 240.527000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1542185616.629553601, 240.527000000]: MoveGroup context initialization complete

You can start planning now!

[ WARN] [1542185623.940113060, 244.175000000]: Timed out waiting for transform from base_link to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.101 timeout was 0.1.

[FetchIt!] [BUG] Gazebo world missing romanoff utility caddy

Description:
The kits (Romanoff utility caddy) are missing from some of the Gazebo worlds in the fetchit_challenge package. I think the model URL is wrong and it's trying to download from the internet.

Steps to reproduce:
Launch any one of these 4 launch files. They all have the same problem:

roslaunch fetchit_challenge fetchit_challenge_arena_montreal2019_highlights.launch
roslaunch fetchit_challenge fetchit_challenge_arena_montreal2019.launch
roslaunch fetchit_challenge main_arena_montreal2019_highlights.launch
roslaunch fetchit_challenge main_arena_montreal2019.launch

In the terminal you see this error message:

[Err] [ModelDatabase.cc:414] Unable to download model[model://romanoff_small_utility_caddy]

It is not a bug on my computer because when I use other launch files like fetchit_challenge_test the model loads correctly.

.

Here is the full output:

[ INFO] [1556064604.854347292]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1556064604.855203264]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.0.6
[ INFO] [1556064604.898350072]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1556064604.899131424]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.0.6
[Wrn] [ModelDatabase.cc:340] Getting models from[http://gazebosim.org/models/]. This may take a few seconds.
[Err] [Node.cc:105] No namespace found
[Err] [Node.cc:105] No namespace found
[Err] [REST.cc:205] Error in REST request

libcurl: (51) SSL: no alternative certificate subject name matches target host name 'api.ignitionfuel.org'
[Err] [Node.cc:105] No namespace found
[Err] [Node.cc:105] No namespace found
[Err] [Node.cc:105] No namespace found
[Err] [Node.cc:105] No namespace found
[Err] [Node.cc:105] No namespace found
[Err] [Node.cc:105] No namespace found
[Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
[Err] [ModelDatabase.cc:414] Unable to download model[model://romanoff_small_utility_caddy]
[Err] [SystemPaths.cc:412] File or path does not exist[""]
Error Code: 11 Msg: Unable to find uri[model://romanoff_small_utility_caddy]
[ INFO] [1556064624.105415954, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1556064624.123060014, 0.039000000]: Physics dynamic reconfigure ready.

Screenshot:
Here is a screenshot from "main arena montreal 2019", on the left you can see empty table.

catkin workspace:

• fetch_gazebo version: Current master branch
• Ubuntu version: 18.04
• ROS version: Melodic

.

When catkin_make fetch_gazebo non-homogeneous workspace error appears

When compiling the entire fetch_gazebo git it gives the following error:

-- ~~ - fetch_simulation (metapackage) WARNING: The CMakeLists.txt of the metapackage 'fetch_simulation' contains non standard content. Use the content of the following file instead: /home/user/simulation_ws/build/catkin_generated/metapackages/fetch_simulation/CMakeLists.txt -- ~~ - fetchit_challenge -- ~~ - fetch_gazebo -- ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ CMake Error at /opt/ros/melodic/share/catkin/cmake/catkin_workspace.cmake:95 (message): This workspace contains non-catkin packages in it, and catkin cannot build a non-homogeneous workspace without isolation. Try the 'catkin_make_isolated' command instead. Call Stack (most recent call first): CMakeLists.txt:63 (catkin_workspace)

Any ideas why this is happening now and how to solve it?
When compiling with the catkin_make-isolated compiles but when launching gives:

Failed to load plugin libfetch_gazebo_plugin.so: libfetch_gazebo_plugin.so: cannot open shared object file: No such file or directory

[FetchIt!] update tables and walls

I'm creating this ticket out of an email from @umhan35

Is your feature request related to a problem? Please describe.

• The tables locations can be updated
• The logos on the tables can be updated
• More details are now known about the arena walls, they will be 40 inch high and white.

Describe the solution you'd like

1. I will get more up to date information about where the tables are located to @RDaneelOlivav
2. There are 4 tables in the arena, and the machine from SCHUNK #61 two of the tables will have SICK and EandM logos on them @RDaneelOlivav we should discuss via email about putting TheConstruct Logo and Fetch on the other.
3. The walls were used in RoboCup 2018 (same venue as ICRA 2019) but they were the same used in RoboCup 2017 in Japan, They're some sort of standard but I don't know which. They're mainly white with aluminium tops and bottom and aluminium posts, but for the simulation all white should be fine. The walls in our office are made out of white foam-core boards with some metal supports behind them, so the simulation, fetch remote environment and real environment in Montreal will always be a little different but close enough for the robot to be able to navigate.

Describe alternatives you've considered
We can just do a bunch of these tickets #61 and #47 all together as one big update.

Additional context
We don't have the covers to the tables yet. I was late sending dimension to EandM.
They've ordered them and have provided these files for how the logos will look:

Currently the layout looks like this: