mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
first implementation of subtask::CurrentState
This commit is contained in:
parent
0bcef6b07c
commit
8af3645af9
@ -13,6 +13,9 @@ public:
|
||||
virtual bool canCompute();
|
||||
|
||||
virtual bool compute();
|
||||
|
||||
protected:
|
||||
bool succeeded_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -1,11 +1,47 @@
|
||||
|
||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <moveit_msgs/GetPlanningScene.h>
|
||||
|
||||
bool
|
||||
moveit::task_constructor::subtasks::CurrentState::canCompute(){
|
||||
return true;
|
||||
return !succeeded_;
|
||||
}
|
||||
|
||||
bool
|
||||
moveit::task_constructor::subtasks::CurrentState::compute(){
|
||||
return false;
|
||||
ros::NodeHandle h;
|
||||
ros::ServiceClient client = h.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
|
||||
client.waitForExistence();
|
||||
|
||||
moveit_msgs::GetPlanningScene::Request req;
|
||||
moveit_msgs::GetPlanningScene::Response res;
|
||||
|
||||
req.components.components =
|
||||
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS
|
||||
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE
|
||||
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS
|
||||
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES
|
||||
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY
|
||||
| moveit_msgs::PlanningSceneComponents::OCTOMAP
|
||||
| moveit_msgs::PlanningSceneComponents::TRANSFORMS
|
||||
| moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX
|
||||
| moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING
|
||||
| moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
|
||||
|
||||
succeeded_= client.call(req, res);
|
||||
|
||||
if(!succeeded_)
|
||||
return false;
|
||||
|
||||
// empty trajectory ref -> this node only produces states
|
||||
robot_trajectory::RobotTrajectoryPtr traj;
|
||||
|
||||
planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model_));
|
||||
ps->setPlanningSceneMsg(res.scene);
|
||||
|
||||
sendBothWays(traj, ps);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user