The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
After days of work, now i can answer this question.
1) put the line above your main:
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
2) put the code below in appropriate position within your main.
ros::NodeHandle nh;
planning_environment::CollisionModels* collisionModels;
planning_models::KinematicState* kinematicState;
collisionModels = new planning_environment::CollisionModels("robot_description");
kinematicState = new planning_models::KinematicState(collisionModels->getKinematicModel());
ros::ServiceClient set_planning_scene_diff_client;
while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
{
ROS_INFO_STREAM("Waiting for planning scene service " << SET_PLANNING_SCENE_DIFF_NAME);
}
set_planning_scene_diff_client = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff> (SET_PLANNING_SCENE_DIFF_NAME);
arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
//THE LINE BELOW IS VERY IMPORTANT!
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res))
{
ROS_WARN("Can't get planning scene");
exit(-1);
}
else
{
ROS_INFO("Successfully get planning scene!");
}
the MOST important thing is the line:
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
| | 2 | No.2 Revision |
After days of work, now i can answer this question.
1) put the line above your main:
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
2) put the code below in appropriate position within your main.
ros::NodeHandle nh;
planning_environment::CollisionModels* collisionModels;
planning_models::KinematicState* kinematicState;
collisionModels = new planning_environment::CollisionModels("robot_description");
kinematicState = new planning_models::KinematicState(collisionModels->getKinematicModel());
ros::ServiceClient set_planning_scene_diff_client;
while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
{
ROS_INFO_STREAM("Waiting for planning scene service " << SET_PLANNING_SCENE_DIFF_NAME);
}
set_planning_scene_diff_client = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff> (SET_PLANNING_SCENE_DIFF_NAME);
arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
//THE LINE BELOW IS VERY IMPORTANT!
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res))
{
ROS_WARN("Can't get planning scene");
exit(-1);
}
else
{
ROS_INFO("Successfully get planning scene!");
}
the MOST important thing is the line:
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
btw, you don't need to change other config files. just use the ones generated by the planning_description_configuration_wizard.
| | 3 | No.3 Revision |
After days of work, now i can answer this question.
1) put the line above your main:
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
2) put the code below in appropriate position within your main.
ros::NodeHandle nh;
planning_environment::CollisionModels* collisionModels;
planning_models::KinematicState* kinematicState;
collisionModels = new planning_environment::CollisionModels("robot_description");
kinematicState = new planning_models::KinematicState(collisionModels->getKinematicModel());
ros::ServiceClient set_planning_scene_diff_client;
while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
{
ROS_INFO_STREAM("Waiting for planning scene service " << SET_PLANNING_SCENE_DIFF_NAME);
}
set_planning_scene_diff_client = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff> (SET_PLANNING_SCENE_DIFF_NAME);
arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
//THE LINE BELOW IS VERY IMPORTANT!
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res))
{
ROS_WARN("Can't get planning scene");
exit(-1);
}
else
{
ROS_INFO("Successfully get planning scene!");
}
collisionModels->revertPlanningScene(kinematicState);
collisionModels->setPlanningScene(planning_scene_res.planning_scene);
the MOST important thing is the line:
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
btw, you don't need to change other config files. just use the ones generated by the planning_description_configuration_wizard.
| | 4 | No.4 Revision |
After days of work, now i can answer this question.
1) put the line above your main:
static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
2) put the code below in appropriate position within your main.
ros::NodeHandle nh;
planning_environment::CollisionModels* collisionModels;
planning_models::KinematicState* kinematicState;
collisionModels = new planning_environment::CollisionModels("robot_description");
kinematicState = new planning_models::KinematicState(collisionModels->getKinematicModel());
ros::ServiceClient set_planning_scene_diff_client;
while(!ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME, ros::Duration(1.0)))
{
ROS_INFO_STREAM("Waiting for planning scene service " << SET_PLANNING_SCENE_DIFF_NAME);
}
set_planning_scene_diff_client = nh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff> (SET_PLANNING_SCENE_DIFF_NAME);
arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
//THE LINE BELOW IS VERY IMPORTANT!
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res))
{
ROS_WARN("Can't get planning scene");
exit(-1);
}
else
{
ROS_INFO("Successfully get planning scene!");
}
collisionModels->revertPlanningScene(kinematicState);
//must call revertPlanningScene() first before calling setPlanningScene(). otherwise you'll be blocked in setPlanningScene()
collisionModels->setPlanningScene(planning_scene_res.planning_scene);
the MOST important thing is the line:
planning_environment::convertKinematicStateToRobotState(*kinematicState, ros::Time::now(), collisionModels->getWorldFrameId(), planning_scene_req.planning_scene_diff.robot_state);
btw, you don't need to change other config files. just use the ones generated by the planning_description_configuration_wizard.
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.