diff --git a/CMakeLists.txt b/CMakeLists.txt index b17fb130..33fb8255 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,6 +34,7 @@ target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES}) add_library(${PROJECT_NAME} src/subtask.cpp src/task.cpp + src/debug.cpp ) add_subdirectory(src/demo) diff --git a/include/moveit_task_constructor/debug.h b/include/moveit_task_constructor/debug.h new file mode 100644 index 00000000..0d1d4e4f --- /dev/null +++ b/include/moveit_task_constructor/debug.h @@ -0,0 +1,16 @@ +#pragma once + +#include +#include + +namespace moveit { namespace task_constructor { + +MOVEIT_CLASS_FORWARD(Task) +MOVEIT_CLASS_FORWARD(SubTrajectory) + +bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, + std::vector& solution, bool wait); + +void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true); + +} } diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index e4a1beb4..d7a12b94 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -11,27 +11,25 @@ #include namespace planning_scene { - MOVEIT_CLASS_FORWARD(PlanningScene); + MOVEIT_CLASS_FORWARD(PlanningScene) } namespace robot_model_loader { - MOVEIT_CLASS_FORWARD(RobotModelLoader); + MOVEIT_CLASS_FORWARD(RobotModelLoader) } namespace planning_pipeline { - MOVEIT_CLASS_FORWARD(PlanningPipeline); + MOVEIT_CLASS_FORWARD(PlanningPipeline) } -namespace robot_trajectory { - MOVEIT_CLASS_FORWARD(RobotTrajectory); -} +namespace moveit { namespace core { + MOVEIT_CLASS_FORWARD(RobotState) +}} namespace moveit { namespace task_constructor { -MOVEIT_CLASS_FORWARD(SubTask); - - -MOVEIT_CLASS_FORWARD(Task); +MOVEIT_CLASS_FORWARD(SubTask) +MOVEIT_CLASS_FORWARD(Task) class Task { public: @@ -44,9 +42,9 @@ public: bool plan(); - bool processSolutions(SolutionCallback& processor); + bool processSolutions(SolutionCallback& processor) const; - void publishPlans(); + const moveit::core::RobotState &getCurrentRobotState() const; void printState(); diff --git a/src/debug.cpp b/src/debug.cpp new file mode 100644 index 00000000..963e9cc9 --- /dev/null +++ b/src/debug.cpp @@ -0,0 +1,42 @@ +#include +#include + +#include +#include +#include + +namespace moveit { namespace task_constructor { + +bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, + std::vector& solution, bool wait){ + dt.trajectory.clear(); + for(auto& t : solution){ + if(t->trajectory){ + dt.trajectory.emplace_back(); + t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back()); + } + } + + std::cout << "publishing solution" << std::endl; + pub.publish(dt); + if (wait) { + std::cout << "Press to continue ..." << std::endl; + getchar(); + } + return true; +} + +void publishAllPlans(const Task &task, const std::string &topic, bool wait){ + ros::NodeHandle n; + ros::Publisher pub = n.advertise(topic, 1, true); + moveit_msgs::DisplayTrajectory dt; + robot_state::robotStateToRobotStateMsg(task.getCurrentRobotState(), dt.trajectory_start); + dt.model_id= task.getCurrentRobotState().getRobotModel()->getName(); + + Task::SolutionCallback processor= std::bind( + &publishSolution, pub, dt, std::placeholders::_1, wait); + + task.processSolutions(processor); +} + +} } diff --git a/src/demo/plan_pick_trixi.cpp b/src/demo/plan_pick_trixi.cpp index e4b987a0..4100153d 100644 --- a/src/demo/plan_pick_trixi.cpp +++ b/src/demo/plan_pick_trixi.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -109,9 +110,7 @@ int main(int argc, char** argv){ t.plan(); - t.publishPlans(); - - //t.printState(); + publishAllPlans(t); return 0; } diff --git a/src/demo/plan_pick_ur5.cpp b/src/demo/plan_pick_ur5.cpp index 7ebba12d..e663e612 100644 --- a/src/demo/plan_pick_ur5.cpp +++ b/src/demo/plan_pick_ur5.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -106,9 +107,7 @@ int main(int argc, char** argv){ t.plan(); - t.publishPlans(); - - //t.printState(); + publishAllPlans(t); return 0; } diff --git a/src/task.cpp b/src/task.cpp index 617470a7..394b92ac 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -4,11 +4,8 @@ #include #include -#include #include -#include -#include #include namespace moveit { namespace task_constructor { @@ -59,6 +56,7 @@ void Task::clear(){ } bool Task::plan(){ + bool computed= true; while(ros::ok() && computed){ computed= false; @@ -89,6 +87,10 @@ void Task::add( SubTaskPtr subtask ){ subtasks_.push_back( subtask ); } +const robot_state::RobotState& Task::getCurrentRobotState() const { + return scene_->getCurrentState(); +} + void Task::printState(){ for( auto& st : subtasks_ ){ std::cout @@ -129,7 +131,7 @@ bool traverseFullTrajectories( } } -bool Task::processSolutions(Task::SolutionCallback& processor ){ +bool Task::processSolutions(Task::SolutionCallback& processor) const { const size_t nr_of_trajectories= subtasks_.size(); std::vector trace; trace.reserve(nr_of_trajectories); @@ -139,35 +141,4 @@ bool Task::processSolutions(Task::SolutionCallback& processor ){ return true; } -namespace { -bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector& solution){ - dt.trajectory.clear(); - for(auto& t : solution){ - if(t->trajectory){ - dt.trajectory.emplace_back(); - t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back()); - } - } - - std::cout << "publishing solution" << std::endl; - pub.publish(dt); - std::cout << "Press to continue ..." << std::endl; - getchar(); - return true; -} -} - -void Task::publishPlans(){ - ros::NodeHandle n; - ros::Publisher pub = n.advertise("task_plan", 1, true); - moveit_msgs::DisplayTrajectory dt; - robot_state::robotStateToRobotStateMsg(scene_->getCurrentState(), dt.trajectory_start); - dt.model_id= scene_->getRobotModel()->getName(); - - Task::SolutionCallback processor= std::bind( - &publishSolution, pub, dt, std::placeholders::_1); - - processSolutions(processor); -} - } }