Skip to content
Snippets Groups Projects
Commit 0ec453e7 authored by KingMaZito's avatar KingMaZito
Browse files

Groot now in parallel

parent 67123e00
Branches
No related tags found
No related merge requests found
...@@ -23,13 +23,13 @@ class Execution : public BT::StatefulActionNode{ ...@@ -23,13 +23,13 @@ class Execution : public BT::StatefulActionNode{
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_; moveit_task_constructor_msgs::ExecuteTaskSolutionGoal ets_;
Moveit_robot* mr_reference_; Moveit_robot* mr_reference_;
ros::Publisher* publisher_reference_; ros::Publisher* publisher_reference_;
std::map<std::string, moveit_msgs::RobotTrajectory>* executions_; std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions_;
std::vector<moveit_task_constructor_msgs::SubTrajectory>::iterator it_; std::vector<moveit_task_constructor_msgs::SubTrajectory>::iterator it_;
public: public:
Execution(const std::string& name, const BT::NodeConfiguration& config); Execution(const std::string& name, const BT::NodeConfiguration& config);
void init(std::map<std::string, moveit_msgs::RobotTrajectory>* executions_, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets); void init(std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions_, ros::Publisher* publisher_reference, Moveit_robot* mr_reference_, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets);
inline static BT::PortsList providedPorts() { return {}; } inline static BT::PortsList providedPorts() { return {}; }
......
...@@ -77,7 +77,7 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -77,7 +77,7 @@ class Moveit_mediator : public Abstract_mediator{
std::map<std::string, std::vector<uint8_t>> acm_; std::map<std::string, std::vector<uint8_t>> acm_;
std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_; std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>> tasks_;
std::map<std::string, moveit_msgs::RobotTrajectory> executions_; std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>> executions_;
...@@ -95,18 +95,19 @@ class Moveit_mediator : public Abstract_mediator{ ...@@ -95,18 +95,19 @@ class Moveit_mediator : public Abstract_mediator{
void status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status); void status_callback(const actionlib_msgs::GoalStatusArray::ConstPtr& status);
void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr); void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene in, Moveit_robot* mr);
void merge_acm(moveit_msgs::PlanningScene& in); void merge_acm(moveit_msgs::PlanningScene& in);
void task_planner(); void task_planner();
void publish_tables(); void publish_tables();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt); void parallel_exec(Moveit_robot& r, moveit_msgs::RobotTrajectory rt);
void manipulate_acm(Moveit_robot* mr, moveit_msgs::PlanningScene& ps);
//void parallel_exec(); //void parallel_exec();
moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); moveit::task_constructor::Task create_Task(Moveit_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
inline std::map<std::string, moveit_msgs::RobotTrajectory>& executions(){return executions_;}; inline std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>& executions(){return executions_;};
inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;}; inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;}; inline void set_tasks(std::multimap<std::string, std::pair<tf2::Vector3, std::queue<moveit_task_constructor_msgs::ExecuteTaskSolutionGoal>>>& tasks) {tasks_ = tasks;};
......
...@@ -22,9 +22,8 @@ BT::NodeStatus Execution::tick() { ...@@ -22,9 +22,8 @@ BT::NodeStatus Execution::tick() {
BT::NodeStatus Execution::onStart(){ BT::NodeStatus Execution::onStart(){
if (it_ != ets_.solution.sub_trajectory.end()){ if (it_ != ets_.solution.sub_trajectory.end()){
executions_->insert_or_assign(mr_reference_->name(), it_->trajectory); std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
ROS_INFO("%i", it_->trajectory.joint_trajectory.points.size()); executions_->insert_or_assign(mr_reference_->name(), pair);
ROS_INFO("%i", it_->trajectory.multi_dof_joint_trajectory.points.size());
it_++; it_++;
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} else { } else {
...@@ -36,9 +35,8 @@ BT::NodeStatus Execution::onStart(){ ...@@ -36,9 +35,8 @@ BT::NodeStatus Execution::onStart(){
// this method until it return something different from RUNNING // this method until it return something different from RUNNING
BT::NodeStatus Execution::onRunning(){ BT::NodeStatus Execution::onRunning(){
if (it_ != ets_.solution.sub_trajectory.end()){ if (it_ != ets_.solution.sub_trajectory.end()){
executions_->insert_or_assign(mr_reference_->name(), it_->trajectory); std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene> pair(it_->trajectory, it_->scene_diff);
ROS_INFO("%i", it_->trajectory.joint_trajectory.points.size()); executions_->insert_or_assign(mr_reference_->name(), pair);
ROS_INFO("%i", it_->trajectory.multi_dof_joint_trajectory.points.size());
it_++; it_++;
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} else { } else {
...@@ -49,7 +47,7 @@ BT::NodeStatus Execution::onRunning(){ ...@@ -49,7 +47,7 @@ BT::NodeStatus Execution::onRunning(){
// callback to execute if the action was aborted by another node // callback to execute if the action was aborted by another node
void Execution::onHalted(){} void Execution::onHalted(){}
void Execution::init(std::map<std::string, moveit_msgs::RobotTrajectory>* executions ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) { void Execution::init(std::map<std::string, std::pair<moveit_msgs::RobotTrajectory, moveit_msgs::PlanningScene>>* executions ,ros::Publisher* publisher_reference, Moveit_robot* mr_reference, moveit_task_constructor_msgs::ExecuteTaskSolutionGoal& ets) {
mr_reference_= mr_reference; mr_reference_= mr_reference;
publisher_reference_ = publisher_reference; publisher_reference_ = publisher_reference;
ets_ = ets; ets_ = ets;
......
This diff is collapsed.
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment