mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
moved plan publishing into debug.cpp
This commit is contained in:
parent
fd62e46828
commit
6d10f7e835
@ -34,6 +34,7 @@ target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES})
|
|||||||
add_library(${PROJECT_NAME}
|
add_library(${PROJECT_NAME}
|
||||||
src/subtask.cpp
|
src/subtask.cpp
|
||||||
src/task.cpp
|
src/task.cpp
|
||||||
|
src/debug.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
add_subdirectory(src/demo)
|
add_subdirectory(src/demo)
|
||||||
|
|||||||
16
include/moveit_task_constructor/debug.h
Normal file
16
include/moveit_task_constructor/debug.h
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <moveit/macros/class_forward.h>
|
||||||
|
#include <moveit_msgs/DisplayTrajectory.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
|
MOVEIT_CLASS_FORWARD(Task)
|
||||||
|
MOVEIT_CLASS_FORWARD(SubTrajectory)
|
||||||
|
|
||||||
|
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
||||||
|
std::vector<SubTrajectory*>& solution, bool wait);
|
||||||
|
|
||||||
|
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);
|
||||||
|
|
||||||
|
} }
|
||||||
@ -11,27 +11,25 @@
|
|||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace robot_model_loader {
|
namespace robot_model_loader {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModelLoader);
|
MOVEIT_CLASS_FORWARD(RobotModelLoader)
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace planning_pipeline {
|
namespace planning_pipeline {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningPipeline);
|
MOVEIT_CLASS_FORWARD(PlanningPipeline)
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace robot_trajectory {
|
namespace moveit { namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotTrajectory);
|
MOVEIT_CLASS_FORWARD(RobotState)
|
||||||
}
|
}}
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(SubTask);
|
MOVEIT_CLASS_FORWARD(SubTask)
|
||||||
|
MOVEIT_CLASS_FORWARD(Task)
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(Task);
|
|
||||||
|
|
||||||
class Task {
|
class Task {
|
||||||
public:
|
public:
|
||||||
@ -44,9 +42,9 @@ public:
|
|||||||
|
|
||||||
bool plan();
|
bool plan();
|
||||||
|
|
||||||
bool processSolutions(SolutionCallback& processor);
|
bool processSolutions(SolutionCallback& processor) const;
|
||||||
|
|
||||||
void publishPlans();
|
const moveit::core::RobotState &getCurrentRobotState() const;
|
||||||
|
|
||||||
void printState();
|
void printState();
|
||||||
|
|
||||||
|
|||||||
42
src/debug.cpp
Normal file
42
src/debug.cpp
Normal file
@ -0,0 +1,42 @@
|
|||||||
|
#include <moveit_task_constructor/debug.h>
|
||||||
|
#include <moveit_task_constructor/task.h>
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||||
|
#include <moveit/robot_state/conversions.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
|
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
||||||
|
std::vector<SubTrajectory*>& 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 <Enter> 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<moveit_msgs::DisplayTrajectory>(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);
|
||||||
|
}
|
||||||
|
|
||||||
|
} }
|
||||||
@ -1,4 +1,5 @@
|
|||||||
#include <moveit_task_constructor/task.h>
|
#include <moveit_task_constructor/task.h>
|
||||||
|
#include <moveit_task_constructor/debug.h>
|
||||||
|
|
||||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||||
@ -109,9 +110,7 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|
||||||
t.publishPlans();
|
publishAllPlans(t);
|
||||||
|
|
||||||
//t.printState();
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,4 +1,5 @@
|
|||||||
#include <moveit_task_constructor/task.h>
|
#include <moveit_task_constructor/task.h>
|
||||||
|
#include <moveit_task_constructor/debug.h>
|
||||||
|
|
||||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||||
@ -106,9 +107,7 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|
||||||
t.publishPlans();
|
publishAllPlans(t);
|
||||||
|
|
||||||
//t.printState();
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
41
src/task.cpp
41
src/task.cpp
@ -4,11 +4,8 @@
|
|||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
|
||||||
#include <moveit_msgs/GetPlanningScene.h>
|
#include <moveit_msgs/GetPlanningScene.h>
|
||||||
#include <moveit_msgs/DisplayTrajectory.h>
|
|
||||||
|
|
||||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
|
||||||
#include <moveit/robot_state/conversions.h>
|
|
||||||
#include <moveit/planning_pipeline/planning_pipeline.h>
|
#include <moveit/planning_pipeline/planning_pipeline.h>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
@ -59,6 +56,7 @@ void Task::clear(){
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool Task::plan(){
|
bool Task::plan(){
|
||||||
|
|
||||||
bool computed= true;
|
bool computed= true;
|
||||||
while(ros::ok() && computed){
|
while(ros::ok() && computed){
|
||||||
computed= false;
|
computed= false;
|
||||||
@ -89,6 +87,10 @@ void Task::add( SubTaskPtr subtask ){
|
|||||||
subtasks_.push_back( subtask );
|
subtasks_.push_back( subtask );
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const robot_state::RobotState& Task::getCurrentRobotState() const {
|
||||||
|
return scene_->getCurrentState();
|
||||||
|
}
|
||||||
|
|
||||||
void Task::printState(){
|
void Task::printState(){
|
||||||
for( auto& st : subtasks_ ){
|
for( auto& st : subtasks_ ){
|
||||||
std::cout
|
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();
|
const size_t nr_of_trajectories= subtasks_.size();
|
||||||
std::vector<SubTrajectory*> trace;
|
std::vector<SubTrajectory*> trace;
|
||||||
trace.reserve(nr_of_trajectories);
|
trace.reserve(nr_of_trajectories);
|
||||||
@ -139,35 +141,4 @@ bool Task::processSolutions(Task::SolutionCallback& processor ){
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace {
|
|
||||||
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt, std::vector<SubTrajectory*>& 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 <Enter> to continue ..." << std::endl;
|
|
||||||
getchar();
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void Task::publishPlans(){
|
|
||||||
ros::NodeHandle n;
|
|
||||||
ros::Publisher pub = n.advertise<moveit_msgs::DisplayTrajectory>("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);
|
|
||||||
}
|
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user