mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
commit
bb1fcad44e
@ -17,7 +17,7 @@ bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
|||||||
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);
|
void publishAllPlans(const Task &task, const std::string &topic = "task_plan", bool wait = true);
|
||||||
|
|
||||||
class NewSolutionPublisher {
|
class NewSolutionPublisher {
|
||||||
std::set<SubTrajectory*> published_;
|
std::set<const SubTrajectory*> published_;
|
||||||
const Task &task_;
|
const Task &task_;
|
||||||
ros::Publisher pub_;
|
ros::Publisher pub_;
|
||||||
|
|
||||||
|
|||||||
@ -10,7 +10,7 @@ namespace moveit { namespace task_constructor {
|
|||||||
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
bool publishSolution(ros::Publisher& pub, moveit_msgs::DisplayTrajectory& dt,
|
||||||
std::vector<SubTrajectory*>& solution, bool wait){
|
std::vector<SubTrajectory*>& solution, bool wait){
|
||||||
dt.trajectory.clear();
|
dt.trajectory.clear();
|
||||||
for(SubTrajectory*& t : solution){
|
for(const SubTrajectory* t : solution){
|
||||||
if(t->trajectory){
|
if(t->trajectory){
|
||||||
dt.trajectory.emplace_back();
|
dt.trajectory.emplace_back();
|
||||||
t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back());
|
t->trajectory->getRobotTrajectoryMsg(dt.trajectory.back());
|
||||||
@ -55,7 +55,7 @@ void NewSolutionPublisher::publish()
|
|||||||
|
|
||||||
Task::SolutionCallback processor = [this,&dt](std::vector<SubTrajectory*>& solution) {
|
Task::SolutionCallback processor = [this,&dt](std::vector<SubTrajectory*>& solution) {
|
||||||
bool all_published = true;
|
bool all_published = true;
|
||||||
for(SubTrajectory*& t : solution){
|
for(const SubTrajectory* t : solution){
|
||||||
auto result = published_.insert(t);
|
auto result = published_.insert(t);
|
||||||
// if t was not yet published, the insertion yields result.second == true
|
// if t was not yet published, the insertion yields result.second == true
|
||||||
all_published &= !result.second;
|
all_published &= !result.second;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user