Problem with Octomap + Moveit! and consistent collisions + dynamic clearing of octomaps.
2,302 views
Skip to first unread message
Parshad Patel
Dec 10, 2014, 2:00:23 PM12/10/14
to moveit...@googlegroups.com
Hi All,
Case1:
I have read the threads here posted previously on integrating PointCloud data into OctoMap and then into MoveIt! and via "/planning_scene" topic or as a "plug-in: occupancy_map_monitor/PointCloudOctomapUpdater" . I am using ASUS Xtion and able to get data from "/camera/depth_registered/points" which I have mapped to "cloud_in" to get the Octomap running. The launch file to launch octomap_server looks like this:
<launch>
<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<param name="resolution" value="0.05" />
<!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
<param name="frame_id" type="string" value="/odom_combined" />
<!-- maximum range to integrate (speedup!) -->
<param name="max_sensor_range" value="1.0" />
<!-- data source to integrate (PointCloud2) -->
<remap from="cloud_in" to="/move_group/camera/depth_registered/points"/>
</node>
</launch>
Kinect.yaml is :
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
point_cloud_topic: /camera/depth_registered/points
max_range: 2.0
point_subsample: 1
padding_offset: 0
padding_scale: 1.0
point_subsample: 1
filtered_cloud_topic: filtered_cloud
Sensor_manager.launch
<launch>
<arg name="kinect" default="true"/>
<group if="$(arg kinect)" >
<rosparam command="load" file="$(find ronameds_moveit_config)/config/kinect.yaml" />
<!-- launch openni to talk to kinect -->
<!-- octomap parameters for moveit -->
<param name="octomap_frame" type="string" value="odom_combined" />
<param name="octomap_resolution" type="double" value="0.02" />
<param name="max_range" type="double" value="5.0" />
<include file="$(find openni2_launch)/launch/openni2.launch" />
</group>
</launch>
I have encountered no problem while launching these files on demo.launch. The robot is successfully loaded in RViz environment and I am able to get the planning i.e no collisions with self or other links. I am also able to visualize octomaps on "/occupied_cells_vis_array". However the topic "/move_group/monitored_planning_scene" remains empty. And for that matter "/planning_scene" and "/planning_scene_world" are empty as well. Due to this, the octomap is ignored during the arm planning of robot.
1) Is my understanding that octomaps are added into move_group, i.e via /planning_scene to robot's environment, by the plug-in correct ? If not please suggest steps I should be following.
Case2:
As a work around to get the octomap into move_group I wrote a node listening to /octomap_binary (also tried with /octomap_full) and published this information to "/planning_scene". I got the Octomap published /move_group/monitored_planning_scene. However, while planning under this condition, the robot is constantly under collision, i.e the start condition is always under collision no matter what, with the octomap although there is no point from octomaps touching/colliding the robot.
I also read that octomaps, environment and links of robot are always considered during collision check by the planner by default and <robot>.srdf contains information for all link collisions. Is there a link, if any, for octomap I can add to *.srdf file ?
As per the tutorials, my listening node looks like:
planning_scene_pub = nh_.advertise<moveit_msgs::PlanningScene>("/planning_scene", 1);
ros::Subscriber octomap_binary_sub = nh_.subscribe("/octomap_binary",10,octomap_to_planning_scene)
void octomap_to_planning_scene(const octomap_msgs::Octomap::ConstPtr msg)
{
planning_scene.world.octomap.octomap = *msg;
planning_scene.world.octomap.octomap.header.frame_id = "world";
planning_scene.world.octomap.header = msg->header;
planning_scene.world.octomap.header.frame_id = "world";
planning_scene.world.octomap.origin.position.x = 0.0;
planning_scene.world.octomap.origin.position.y = 0.0;
planning_scene.world.octomap.origin.position.z = 0.0;
planning_scene.world.octomap.origin.orientation.x = 0.0;
planning_scene.world.octomap.origin.orientation.x = 0.0;
planning_scene.world.octomap.origin.orientation.x = 0.0;
planning_scene.world.octomap.origin.orientation.x = 0.0;
//planning_scene.is_diff = true;
planning_scene_pub.publish(planning_scene);
}
2) Is there anything I am missing ? Also what is the use of planning_scene.is_diff ? If I set it true, the octomaps are ignored while planning.
Case3:
Morever while shutting the external node, I have been clearing the octomap and all topics, /planning_scene , /move_group/monitored_planning scene are cleared. However the arm still remains under collision.
The shutdown method looks like:
octomap_msgs::Octomap empty;
void shutdown(int sig)
{
planning_scene.world.octomap.octomap = empty;
planning_scene_pub.publish(planning_scene);
ros::Duration sleep_time(1);
sleep_time.sleep();
ros::shutdown();
}
3) How to overcome this problem ?
_______________________________________
I have attached screenshots for the robot:
Using marker_array is for Case1,
With Octomap+move_group is for Case2.
Clearing Octomap is for Case3
Many thanks !
Case1:
I have read the threads here posted previously on integrating PointCloud data into OctoMap and then into MoveIt! and via "/planning_scene" topic or as a "plug-in: occupancy_map_monitor/PointCloudOctomapUpdater" . I am using ASUS Xtion and able to get data from "/camera/depth_registered/points" which I have mapped to "cloud_in" to get the Octomap running. The launch file to launch octomap_server looks like this:
<launch>
<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
<param name="resolution" value="0.05" />
<!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
<param name="frame_id" type="string" value="/odom_combined" />
<!-- maximum range to integrate (speedup!) -->
<param name="max_sensor_range" value="1.0" />
<!-- data source to integrate (PointCloud2) -->
<remap from="cloud_in" to="/move_group/camera/depth_registered/points"/>
</node>
</launch>
Kinect.yaml is :
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
point_cloud_topic: /camera/depth_registered/points
max_range: 2.0
point_subsample: 1
padding_offset: 0
padding_scale: 1.0
point_subsample: 1
filtered_cloud_topic: filtered_cloud
Sensor_manager.launch
<launch>
<arg name="kinect" default="true"/>
<group if="$(arg kinect)" >
<rosparam command="load" file="$(find ronameds_moveit_config)/config/kinect.yaml" />
<!-- launch openni to talk to kinect -->
<!-- octomap parameters for moveit -->
<param name="octomap_frame" type="string" value="odom_combined" />
<param name="octomap_resolution" type="double" value="0.02" />
<param name="max_range" type="double" value="5.0" />
<include file="$(find openni2_launch)/launch/openni2.launch" />
</group>
</launch>
I have encountered no problem while launching these files on demo.launch. The robot is successfully loaded in RViz environment and I am able to get the planning i.e no collisions with self or other links. I am also able to visualize octomaps on "/occupied_cells_vis_array". However the topic "/move_group/monitored_planning_scene" remains empty. And for that matter "/planning_scene" and "/planning_scene_world" are empty as well. Due to this, the octomap is ignored during the arm planning of robot.
1) Is my understanding that octomaps are added into move_group, i.e via /planning_scene to robot's environment, by the plug-in correct ? If not please suggest steps I should be following.
Case2:
As a work around to get the octomap into move_group I wrote a node listening to /octomap_binary (also tried with /octomap_full) and published this information to "/planning_scene". I got the Octomap published /move_group/monitored_planning_scene. However, while planning under this condition, the robot is constantly under collision, i.e the start condition is always under collision no matter what, with the octomap although there is no point from octomaps touching/colliding the robot.
I also read that octomaps, environment and links of robot are always considered during collision check by the planner by default and <robot>.srdf contains information for all link collisions. Is there a link, if any, for octomap I can add to *.srdf file ?
As per the tutorials, my listening node looks like:
planning_scene_pub = nh_.advertise<moveit_msgs::PlanningScene>("/planning_scene", 1);
ros::Subscriber octomap_binary_sub = nh_.subscribe("/octomap_binary",10,octomap_to_planning_scene)
void octomap_to_planning_scene(const octomap_msgs::Octomap::ConstPtr msg)
{
planning_scene.world.octomap.octomap = *msg;
planning_scene.world.octomap.octomap.header.frame_id = "world";
planning_scene.world.octomap.header = msg->header;
planning_scene.world.octomap.header.frame_id = "world";
planning_scene.world.octomap.origin.position.x = 0.0;
planning_scene.world.octomap.origin.position.y = 0.0;
planning_scene.world.octomap.origin.position.z = 0.0;
planning_scene.world.octomap.origin.orientation.x = 0.0;
planning_scene.world.octomap.origin.orientation.x = 0.0;
planning_scene.world.octomap.origin.orientation.x = 0.0;
planning_scene.world.octomap.origin.orientation.x = 0.0;
//planning_scene.is_diff = true;
planning_scene_pub.publish(planning_scene);
}
2) Is there anything I am missing ? Also what is the use of planning_scene.is_diff ? If I set it true, the octomaps are ignored while planning.
Case3:
Morever while shutting the external node, I have been clearing the octomap and all topics, /planning_scene , /move_group/monitored_planning scene are cleared. However the arm still remains under collision.
The shutdown method looks like:
octomap_msgs::Octomap empty;
void shutdown(int sig)
{
planning_scene.world.octomap.octomap = empty;
planning_scene_pub.publish(planning_scene);
ros::Duration sleep_time(1);
sleep_time.sleep();
ros::shutdown();
}
3) How to overcome this problem ?
_______________________________________
I have attached screenshots for the robot:
Using marker_array is for Case1,
With Octomap+move_group is for Case2.
Clearing Octomap is for Case3
Many thanks !
Parshad Patel
Dec 10, 2014, 3:19:11 PM12/10/14
to moveit...@googlegroups.com
Anyone? Please help me.
Sachin Chitta
Dec 10, 2014, 7:02:39 PM12/10/14
to Parshad Patel, moveit-users
Can you check move_group.launch and make sure that the sensor_manager.launch file used in there is the same as the one that you have filled in (check path and file name)? It might be looking for sensor_manager.launch.xml - in your email you have it listed as looking for "Sensor_manager.launch". You don't need the octomap server separately to do this. Move group should be creating and maintaining its own octomap if everything works right.
SachinParshad Patel
Dec 11, 2014, 12:04:58 PM12/11/14
to moveit...@googlegroups.com, parsh...@gmail.com
Hi Sachin,
I am sorry, it was sensor_manager.launch.xml instead of sensor_manager.launch.
The problem still persists, I am not able to load the octomap into the move_group. Here is the output for demo.launch
>>
started roslaunch server http://rona2-host:37393/
SUMMARY
========
PARAMETERS
* /move_group/allow_trajectory_execution
* /move_group/allowed_execution_duration_scaling
* /move_group/allowed_goal_duration_margin
* /move_group/camera/camera_nodelet_manager/num_worker_threads
* /move_group/camera/depth_rectify_depth/interpolation
* /move_group/camera/driver/auto_exposure
* /move_group/camera/driver/auto_white_balance
* /move_group/camera/driver/color_depth_synchronization
* /move_group/camera/driver/depth_camera_info_url
* /move_group/camera/driver/depth_frame_id
* /move_group/camera/driver/depth_registration
* /move_group/camera/driver/device_id
* /move_group/camera/driver/rgb_camera_info_url
* /move_group/camera/driver/rgb_frame_id
* /move_group/capabilities
* /move_group/controller_list
* /move_group/jiggle_fraction
* /move_group/l_arm/longest_valid_segment_fraction
* /move_group/l_arm/planner_configs
* /move_group/l_arm/projection_evaluator
* /move_group/max_range
* /move_group/max_safe_path_cost
* /move_group/moveit_controller_manager
* /move_group/moveit_manage_controllers
* /move_group/octomap_frame
* /move_group/octomap_resolution
* /move_group/planner_configs/BKPIECEkConfigDefault/type
* /move_group/planner_configs/ESTkConfigDefault/type
* /move_group/planner_configs/KPIECEkConfigDefault/type
* /move_group/planner_configs/LBKPIECEkConfigDefault/type
* /move_group/planner_configs/PRMkConfigDefault/type
* /move_group/planner_configs/PRMstarkConfigDefault/type
* /move_group/planner_configs/RRTConnectkConfigDefault/type
* /move_group/planner_configs/RRTkConfigDefault/type
* /move_group/planner_configs/RRTstarkConfigDefault/type
* /move_group/planner_configs/SBLkConfigDefault/type
* /move_group/planner_configs/TRRTkConfigDefault/type
* /move_group/planning_plugin
* /move_group/planning_scene_monitor/publish_geometry_updates
* /move_group/planning_scene_monitor/publish_planning_scene
* /move_group/planning_scene_monitor/publish_state_updates
* /move_group/planning_scene_monitor/publish_transforms_updates
* /move_group/request_adapters
* /move_group/sensors
* /move_group/start_state_max_bounds_error
* /robot_description
* /robot_description_kinematics/l_arm/kinematics_solver
* /robot_description_kinematics/l_arm/kinematics_solver_attempts
* /robot_description_kinematics/l_arm/kinematics_solver_search_resolution
* /robot_description_kinematics/l_arm/kinematics_solver_timeout
* /robot_description_planning/joint_limits/l_elbow_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_elbow_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_elbow_joint/max_acceleration
* /robot_description_planning/joint_limits/l_elbow_joint/max_velocity
* /robot_description_planning/joint_limits/l_hand_f1_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_hand_f1_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_hand_f1_joint/max_acceleration
* /robot_description_planning/joint_limits/l_hand_f1_joint/max_velocity
* /robot_description_planning/joint_limits/l_hand_f2_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_hand_f2_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_hand_f2_joint/max_acceleration
* /robot_description_planning/joint_limits/l_hand_f2_joint/max_velocity
* /robot_description_planning/joint_limits/l_shoulder_lift_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_shoulder_lift_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_shoulder_lift_joint/max_acceleration
* /robot_description_planning/joint_limits/l_shoulder_lift_joint/max_velocity
* /robot_description_planning/joint_limits/l_shoulder_pan_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_shoulder_pan_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_shoulder_pan_joint/max_acceleration
* /robot_description_planning/joint_limits/l_shoulder_pan_joint/max_velocity
* /robot_description_planning/joint_limits/l_shoulder_roll_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_shoulder_roll_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_shoulder_roll_joint/max_acceleration
* /robot_description_planning/joint_limits/l_shoulder_roll_joint/max_velocity
* /robot_description_planning/joint_limits/l_wrist_pan_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_wrist_pan_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_wrist_pan_joint/max_acceleration
* /robot_description_planning/joint_limits/l_wrist_pan_joint/max_velocity
* /robot_description_planning/joint_limits/l_wrist_roll_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_wrist_roll_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_wrist_roll_joint/max_acceleration
* /robot_description_planning/joint_limits/l_wrist_roll_joint/max_velocity
* /robot_description_semantic
* /rosdistro
* /rosversion
* /rviz_rona2_host_27994_8709268197595171228/l_arm/kinematics_solver
* /rviz_rona2_host_27994_8709268197595171228/l_arm/kinematics_solver_attempts
* /rviz_rona2_host_27994_8709268197595171228/l_arm/kinematics_solver_search_resolution
* /rviz_rona2_host_27994_8709268197595171228/l_arm/kinematics_solver_timeout
NODES
/move_group/camera/
camera_nodelet_manager (nodelet/nodelet)
depth_metric (nodelet/nodelet)
depth_metric_rect (nodelet/nodelet)
depth_points (nodelet/nodelet)
depth_rectify_depth (nodelet/nodelet)
driver (nodelet/nodelet)
points_xyzrgb_sw_registered (nodelet/nodelet)
rectify_color (nodelet/nodelet)
register_depth_rgb (nodelet/nodelet)
/move_group/
camera_base_link (tf/static_transform_publisher)
camera_base_link1 (tf/static_transform_publisher)
camera_base_link2 (tf/static_transform_publisher)
camera_base_link3 (tf/static_transform_publisher)
/
move_group (moveit_ros_move_group/move_group)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rviz_rona2_host_27994_8709268197595171228 (rviz/rviz)
virtual_joint_broadcaster_0 (tf/static_transform_publisher)
ROS_MASTER_URI=http://localhost:11311
core service [/rosout] found
process[virtual_joint_broadcaster_0-1]: started with pid [28012]
process[robot_state_publisher-2]: started with pid [28023]
/opt/ros/hydro/lib/robot_state_publisher/robot_state_publisher
process[move_group/camera/camera_nodelet_manager-3]: started with pid [28041]
[ INFO] [1418316564.138619278]: Initializing nodelet with 4 worker threads.
process[move_group/camera/driver-4]: started with pid [28062]
Warning: USB events thread - failed to set priority. This might cause loss of data...
process[move_group/camera/rectify_color-5]: started with pid [28079]
[ INFO] [1418316564.736582066]: Device "1d27/0600@1/40" with serial number "1205100161" connected
Warning: USB events thread - failed to set priority. This might cause loss of data...
process[move_group/camera/depth_rectify_depth-6]: started with pid [28104]
process[move_group/camera/depth_metric_rect-7]: started with pid [28172]
process[move_group/camera/depth_metric-8]: started with pid [28188]
process[move_group/camera/depth_points-9]: started with pid [28202]
process[move_group/camera/register_depth_rgb-10]: started with pid [28249]
process[move_group/camera/points_xyzrgb_sw_registered-11]: started with pid [28315]
process[move_group/camera_base_link-12]: started with pid [28378]
process[move_group/camera_base_link1-13]: started with pid [28426]
process[move_group/camera_base_link2-14]: started with pid [28479]
process[move_group/camera_base_link3-15]: started with pid [28517]
process[move_group-16]: started with pid [28571]
[ INFO] [1418316567.981667121]: Loading robot model 'ronameds'...
process[rviz_rona2_host_27994_8709268197595171228-17]: started with pid [28621]
[ INFO] [1418316568.538599394]: rviz version 1.10.18
[ INFO] [1418316568.538897979]: compiled against OGRE version 1.7.4 (Cthugha)
[ INFO] [1418316568.903038016]: Stereo is NOT SUPPORTED
[ INFO] [1418316568.903440019]: OpenGl version: 3 (GLSL 1.3).
[ INFO] [1418316570.960532798]: Loading robot model 'ronameds'...
[ INFO] [1418316575.136025104]: Loading robot model 'ronameds'...
[ INFO] [1418316575.462344637]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1418316575.473219695]: MoveGroup debug mode is ON
Starting context monitors...
[ INFO] [1418316575.473913790]: Starting scene monitor
[ INFO] [1418316575.476837975]: Listening to '/planning_scene'
[ INFO] [1418316575.477028260]: Starting world geometry monitor
[ INFO] [1418316575.479537785]: Listening to '/collision_object' using message notifier with target frame '/odom_combined '
[ INFO] [1418316575.482545479]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1418316578.024219092]: Loading robot model 'ronameds'...
[ INFO] [1418316583.368304301]: Starting scene monitor
[ INFO] [1418316583.370386154]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1418316583.373606386]: waitForService: Service [/get_planning_scene] has not been advertised, waiting...
[ WARN] [1418316588.375592361]: Failed to call service /get_planning_scene, have you launched move_group? at /tmp/buildd/ros-hydro-moveit-ros-planning-0.5.19-0precise-20141027-2216/planning_scene_monitor/src/planning_scene_monitor.cpp:438
[ INFO] [1418316592.783631807]: Constructing new MoveGroup connection for group 'l_arm'
[ERROR] [1418316622.825702814]: Unable to connect to move_group action server within allotted time (2)
[ INFO] [1418316622.827285926]: Constructing new MoveGroup connection for group 'l_arm'
[ INFO] [1418316631.650761855]: Listening to '/camera/depth_registered/points' using message filter with target frame '/odom_combined '
[ INFO] [1418316631.653785080]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1418316632.171160286]: Initializing OMPL interface using ROS parameters
[ WARN] [1418316632.188007461]: Manifest not found in folder '~/constraints_approximation_database'. Not loading constraint approximations.
[ INFO] [1418316632.188129869]:
[ INFO] [1418316632.204994927]: Using planning interface 'OMPL'
[ INFO] [1418316632.362734764]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1418316632.363649414]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1418316632.364591617]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1418316632.365563597]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1418316632.366637791]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1418316632.367537751]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1418316632.367659334]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1418316632.367702891]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1418316632.367753577]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1418316632.367800385]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1418316632.367844938]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1418316632.536562034]: Fake controller 'l_arm_controller' with joints [ l_shoulder_lift_joint l_shoulder_roll_joint l_shoulder_pan_joint l_elbow_joint l_wrist_pan_joint l_wrist_roll_joint ]
[ INFO] [1418316632.537948930]: Fake controller 'l_ee_controller' with joints [ l_hand_f1_joint l_hand_f2_joint ]
[ INFO] [1418316632.538113790]: Returned 2 controllers in list
[ INFO] [1418316632.551355445]: Trajectory execution is managing controllers
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
[ INFO] [1418316632.916637265]:
********************************************************
* MoveGroup using:
* - CartesianPathService
* - ExecutePathService
* - KinematicsService
* - MoveAction
* - PickPlaceAction
* - MotionPlanService
* - QueryPlannersService
* - StateValidationService
* - GetPlanningSceneService
********************************************************
[ INFO] [1418316632.916722597]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1418316632.916752229]: MoveGroup context initialization complete
All is well! Everyone is happy! You can start planning now!
There is a warning for /get_planning_scene service not allotted and an error for move_group action server not alloted, which assume are to be disregarded.
Moreover looking to the parameters, the /move_group/sensors is loaded which means occupancy_map_monitor/PointCloudOctomapUpdater has been loaded. Do I still need to start octomap_server to enable working of this plugin ?
I am sorry, it was sensor_manager.launch.xml instead of sensor_manager.launch.
The problem still persists, I am not able to load the octomap into the move_group. Here is the output for demo.launch
>>
started roslaunch server http://rona2-host:37393/
SUMMARY
========
PARAMETERS
* /move_group/allow_trajectory_execution
* /move_group/allowed_execution_duration_scaling
* /move_group/allowed_goal_duration_margin
* /move_group/camera/camera_nodelet_manager/num_worker_threads
* /move_group/camera/depth_rectify_depth/interpolation
* /move_group/camera/driver/auto_exposure
* /move_group/camera/driver/auto_white_balance
* /move_group/camera/driver/color_depth_synchronization
* /move_group/camera/driver/depth_camera_info_url
* /move_group/camera/driver/depth_frame_id
* /move_group/camera/driver/depth_registration
* /move_group/camera/driver/device_id
* /move_group/camera/driver/rgb_camera_info_url
* /move_group/camera/driver/rgb_frame_id
* /move_group/capabilities
* /move_group/controller_list
* /move_group/jiggle_fraction
* /move_group/l_arm/longest_valid_segment_fraction
* /move_group/l_arm/planner_configs
* /move_group/l_arm/projection_evaluator
* /move_group/max_range
* /move_group/max_safe_path_cost
* /move_group/moveit_controller_manager
* /move_group/moveit_manage_controllers
* /move_group/octomap_frame
* /move_group/octomap_resolution
* /move_group/planner_configs/BKPIECEkConfigDefault/type
* /move_group/planner_configs/ESTkConfigDefault/type
* /move_group/planner_configs/KPIECEkConfigDefault/type
* /move_group/planner_configs/LBKPIECEkConfigDefault/type
* /move_group/planner_configs/PRMkConfigDefault/type
* /move_group/planner_configs/PRMstarkConfigDefault/type
* /move_group/planner_configs/RRTConnectkConfigDefault/type
* /move_group/planner_configs/RRTkConfigDefault/type
* /move_group/planner_configs/RRTstarkConfigDefault/type
* /move_group/planner_configs/SBLkConfigDefault/type
* /move_group/planner_configs/TRRTkConfigDefault/type
* /move_group/planning_plugin
* /move_group/planning_scene_monitor/publish_geometry_updates
* /move_group/planning_scene_monitor/publish_planning_scene
* /move_group/planning_scene_monitor/publish_state_updates
* /move_group/planning_scene_monitor/publish_transforms_updates
* /move_group/request_adapters
* /move_group/sensors
* /move_group/start_state_max_bounds_error
* /robot_description
* /robot_description_kinematics/l_arm/kinematics_solver
* /robot_description_kinematics/l_arm/kinematics_solver_attempts
* /robot_description_kinematics/l_arm/kinematics_solver_search_resolution
* /robot_description_kinematics/l_arm/kinematics_solver_timeout
* /robot_description_planning/joint_limits/l_elbow_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_elbow_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_elbow_joint/max_acceleration
* /robot_description_planning/joint_limits/l_elbow_joint/max_velocity
* /robot_description_planning/joint_limits/l_hand_f1_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_hand_f1_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_hand_f1_joint/max_acceleration
* /robot_description_planning/joint_limits/l_hand_f1_joint/max_velocity
* /robot_description_planning/joint_limits/l_hand_f2_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_hand_f2_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_hand_f2_joint/max_acceleration
* /robot_description_planning/joint_limits/l_hand_f2_joint/max_velocity
* /robot_description_planning/joint_limits/l_shoulder_lift_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_shoulder_lift_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_shoulder_lift_joint/max_acceleration
* /robot_description_planning/joint_limits/l_shoulder_lift_joint/max_velocity
* /robot_description_planning/joint_limits/l_shoulder_pan_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_shoulder_pan_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_shoulder_pan_joint/max_acceleration
* /robot_description_planning/joint_limits/l_shoulder_pan_joint/max_velocity
* /robot_description_planning/joint_limits/l_shoulder_roll_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_shoulder_roll_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_shoulder_roll_joint/max_acceleration
* /robot_description_planning/joint_limits/l_shoulder_roll_joint/max_velocity
* /robot_description_planning/joint_limits/l_wrist_pan_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_wrist_pan_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_wrist_pan_joint/max_acceleration
* /robot_description_planning/joint_limits/l_wrist_pan_joint/max_velocity
* /robot_description_planning/joint_limits/l_wrist_roll_joint/has_acceleration_limits
* /robot_description_planning/joint_limits/l_wrist_roll_joint/has_velocity_limits
* /robot_description_planning/joint_limits/l_wrist_roll_joint/max_acceleration
* /robot_description_planning/joint_limits/l_wrist_roll_joint/max_velocity
* /robot_description_semantic
* /rosdistro
* /rosversion
* /rviz_rona2_host_27994_8709268197595171228/l_arm/kinematics_solver
* /rviz_rona2_host_27994_8709268197595171228/l_arm/kinematics_solver_attempts
* /rviz_rona2_host_27994_8709268197595171228/l_arm/kinematics_solver_search_resolution
* /rviz_rona2_host_27994_8709268197595171228/l_arm/kinematics_solver_timeout
NODES
/move_group/camera/
camera_nodelet_manager (nodelet/nodelet)
depth_metric (nodelet/nodelet)
depth_metric_rect (nodelet/nodelet)
depth_points (nodelet/nodelet)
depth_rectify_depth (nodelet/nodelet)
driver (nodelet/nodelet)
points_xyzrgb_sw_registered (nodelet/nodelet)
rectify_color (nodelet/nodelet)
register_depth_rgb (nodelet/nodelet)
/move_group/
camera_base_link (tf/static_transform_publisher)
camera_base_link1 (tf/static_transform_publisher)
camera_base_link2 (tf/static_transform_publisher)
camera_base_link3 (tf/static_transform_publisher)
/
move_group (moveit_ros_move_group/move_group)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
rviz_rona2_host_27994_8709268197595171228 (rviz/rviz)
virtual_joint_broadcaster_0 (tf/static_transform_publisher)
ROS_MASTER_URI=http://localhost:11311
core service [/rosout] found
process[virtual_joint_broadcaster_0-1]: started with pid [28012]
process[robot_state_publisher-2]: started with pid [28023]
/opt/ros/hydro/lib/robot_state_publisher/robot_state_publisher
process[move_group/camera/camera_nodelet_manager-3]: started with pid [28041]
[ INFO] [1418316564.138619278]: Initializing nodelet with 4 worker threads.
process[move_group/camera/driver-4]: started with pid [28062]
Warning: USB events thread - failed to set priority. This might cause loss of data...
process[move_group/camera/rectify_color-5]: started with pid [28079]
[ INFO] [1418316564.736582066]: Device "1d27/0600@1/40" with serial number "1205100161" connected
Warning: USB events thread - failed to set priority. This might cause loss of data...
process[move_group/camera/depth_rectify_depth-6]: started with pid [28104]
process[move_group/camera/depth_metric_rect-7]: started with pid [28172]
process[move_group/camera/depth_metric-8]: started with pid [28188]
process[move_group/camera/depth_points-9]: started with pid [28202]
process[move_group/camera/register_depth_rgb-10]: started with pid [28249]
process[move_group/camera/points_xyzrgb_sw_registered-11]: started with pid [28315]
process[move_group/camera_base_link-12]: started with pid [28378]
process[move_group/camera_base_link1-13]: started with pid [28426]
process[move_group/camera_base_link2-14]: started with pid [28479]
process[move_group/camera_base_link3-15]: started with pid [28517]
process[move_group-16]: started with pid [28571]
[ INFO] [1418316567.981667121]: Loading robot model 'ronameds'...
process[rviz_rona2_host_27994_8709268197595171228-17]: started with pid [28621]
[ INFO] [1418316568.538599394]: rviz version 1.10.18
[ INFO] [1418316568.538897979]: compiled against OGRE version 1.7.4 (Cthugha)
[ INFO] [1418316568.903038016]: Stereo is NOT SUPPORTED
[ INFO] [1418316568.903440019]: OpenGl version: 3 (GLSL 1.3).
[ INFO] [1418316570.960532798]: Loading robot model 'ronameds'...
[ INFO] [1418316575.136025104]: Loading robot model 'ronameds'...
[ INFO] [1418316575.462344637]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1418316575.473219695]: MoveGroup debug mode is ON
Starting context monitors...
[ INFO] [1418316575.473913790]: Starting scene monitor
[ INFO] [1418316575.476837975]: Listening to '/planning_scene'
[ INFO] [1418316575.477028260]: Starting world geometry monitor
[ INFO] [1418316575.479537785]: Listening to '/collision_object' using message notifier with target frame '/odom_combined '
[ INFO] [1418316575.482545479]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1418316578.024219092]: Loading robot model 'ronameds'...
[ INFO] [1418316583.368304301]: Starting scene monitor
[ INFO] [1418316583.370386154]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1418316583.373606386]: waitForService: Service [/get_planning_scene] has not been advertised, waiting...
[ WARN] [1418316588.375592361]: Failed to call service /get_planning_scene, have you launched move_group? at /tmp/buildd/ros-hydro-moveit-ros-planning-0.5.19-0precise-20141027-2216/planning_scene_monitor/src/planning_scene_monitor.cpp:438
[ INFO] [1418316592.783631807]: Constructing new MoveGroup connection for group 'l_arm'
[ERROR] [1418316622.825702814]: Unable to connect to move_group action server within allotted time (2)
[ INFO] [1418316622.827285926]: Constructing new MoveGroup connection for group 'l_arm'
[ INFO] [1418316631.650761855]: Listening to '/camera/depth_registered/points' using message filter with target frame '/odom_combined '
[ INFO] [1418316631.653785080]: Listening to '/attached_collision_object' for attached collision objects
Context monitors started.
[ INFO] [1418316632.171160286]: Initializing OMPL interface using ROS parameters
[ WARN] [1418316632.188007461]: Manifest not found in folder '~/constraints_approximation_database'. Not loading constraint approximations.
[ INFO] [1418316632.188129869]:
[ INFO] [1418316632.204994927]: Using planning interface 'OMPL'
[ INFO] [1418316632.362734764]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1418316632.363649414]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1418316632.364591617]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1418316632.365563597]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1418316632.366637791]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1418316632.367537751]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1418316632.367659334]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1418316632.367702891]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1418316632.367753577]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1418316632.367800385]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1418316632.367844938]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1418316632.536562034]: Fake controller 'l_arm_controller' with joints [ l_shoulder_lift_joint l_shoulder_roll_joint l_shoulder_pan_joint l_elbow_joint l_wrist_pan_joint l_wrist_roll_joint ]
[ INFO] [1418316632.537948930]: Fake controller 'l_ee_controller' with joints [ l_hand_f1_joint l_hand_f2_joint ]
[ INFO] [1418316632.538113790]: Returned 2 controllers in list
[ INFO] [1418316632.551355445]: Trajectory execution is managing controllers
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
[ INFO] [1418316632.916637265]:
********************************************************
* MoveGroup using:
* - CartesianPathService
* - ExecutePathService
* - KinematicsService
* - MoveAction
* - PickPlaceAction
* - MotionPlanService
* - QueryPlannersService
* - StateValidationService
* - GetPlanningSceneService
********************************************************
[ INFO] [1418316632.916722597]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1418316632.916752229]: MoveGroup context initialization complete
All is well! Everyone is happy! You can start planning now!
There is a warning for /get_planning_scene service not allotted and an error for move_group action server not alloted, which assume are to be disregarded.
Moreover looking to the parameters, the /move_group/sensors is loaded which means occupancy_map_monitor/PointCloudOctomapUpdater has been loaded. Do I still need to start octomap_server to enable working of this plugin ?
Parshad Patel
Dec 11, 2014, 4:16:39 PM12/11/14
to moveit...@googlegroups.com, parsh...@gmail.com
Update:
I have been able to launch everything. I figured that the plug-in wasn't subscribing to the proper topic for camera's point-cloud data. However I have started getting another problem:
After the demo.launch is done, following appears within one second:
[ERROR] [1418331560.512872831]: Transform error: "l_wrist_pan_link" passed to lookupTransform argument source_frame does not exist.
[ERROR] [1418331560.512963714]: Transform cache was not updated. Self-filtering may fail.
This error appears in "http://moveit.ros.org/doxygen/pointcloud__octomap__updater_8cpp_source.html"
Is this a tf problem ? Or is it something else ?
Thanking you.
I have been able to launch everything. I figured that the plug-in wasn't subscribing to the proper topic for camera's point-cloud data. However I have started getting another problem:
After the demo.launch is done, following appears within one second:
[ERROR] [1418331560.512872831]: Transform error: "l_wrist_pan_link" passed to lookupTransform argument source_frame does not exist.
[ERROR] [1418331560.512963714]: Transform cache was not updated. Self-filtering may fail.
This error appears in "http://moveit.ros.org/doxygen/pointcloud__octomap__updater_8cpp_source.html"
Is this a tf problem ? Or is it something else ?
Thanking you.
Parshad
Dec 11, 2014, 6:25:01 PM12/11/14
to moveit...@googlegroups.com, parsh...@gmail.com
Update: Resolved tf problem and now octomap is detected as collision.
Somehow the joint_state_publisher wasn't publishing the joint information to tf hence the error.
Thanks a lot Sachin for your help.
Video:
https://drive.google.com/open?id=0B4ZBrGTGBU5iRXpYaTd0S1JJTTA&authuser=0
Somehow the joint_state_publisher wasn't publishing the joint information to tf hence the error.
Thanks a lot Sachin for your help.
Video:
https://drive.google.com/open?id=0B4ZBrGTGBU5iRXpYaTd0S1JJTTA&authuser=0
Reply all
Reply to author
Forward
0 new messages