mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
replace std::cout with ros console
This commit is contained in:
parent
8184156575
commit
2ea463fff4
@ -104,7 +104,7 @@ public:
|
||||
/// reset, init scene (if not yet done), and init all stages, then start planning
|
||||
bool plan();
|
||||
/// print current task state (number of found solutions and propagated states) to std::cout
|
||||
static void printState(const Task &t);
|
||||
void printState(std::ostream &os = std::cout) const;
|
||||
|
||||
size_t numSolutions() const override;
|
||||
void processSolutions(const ContainerBase::SolutionProcessor &processor) const override;
|
||||
|
||||
@ -393,10 +393,10 @@ bool SerialContainer::compute()
|
||||
for(const auto& stage : pimpl()->children()) {
|
||||
if(!stage->pimpl()->canCompute())
|
||||
continue;
|
||||
std::cout << "Computing stage '" << stage->name() << "':" << std::endl;
|
||||
ROS_INFO("Computing stage '%s'", stage->name().c_str());
|
||||
bool success = stage->pimpl()->compute();
|
||||
computed = true;
|
||||
std::cout << (success ? "succeeded" : "failed") << std::endl;
|
||||
ROS_INFO("Stage '%s': %s", stage->name().c_str(), success ? "succeeded" : "failed");
|
||||
}
|
||||
return computed;
|
||||
}
|
||||
|
||||
@ -155,7 +155,6 @@ void Introspection::publishAllSolutions(bool wait)
|
||||
moveit_task_constructor_msgs::Solution msg;
|
||||
|
||||
Stage::SolutionProcessor processor = [this, &msg, wait](const SolutionBase& s) {
|
||||
std::cout << "publishing solution with cost: " << s.cost() << std::endl;
|
||||
msg.sub_solution.clear();
|
||||
msg.sub_trajectory.clear();
|
||||
fillSolution(msg, s);
|
||||
|
||||
@ -386,7 +386,6 @@ void PropagatingEitherWay::sendForward(const InterfaceState& from,
|
||||
InterfaceState&& to,
|
||||
SubTrajectory&& t) {
|
||||
auto impl = pimpl();
|
||||
std::cout << "sending state forward" << std::endl;
|
||||
SubTrajectory &trajectory = impl->addTrajectory(std::move(t));
|
||||
trajectory.setStartState(from);
|
||||
impl->nextStarts()->add(std::move(to), &trajectory, NULL);
|
||||
@ -397,7 +396,6 @@ void PropagatingEitherWay::sendBackward(InterfaceState&& from,
|
||||
const InterfaceState& to,
|
||||
SubTrajectory&& t) {
|
||||
auto impl = pimpl();
|
||||
std::cout << "sending state backward" << std::endl;
|
||||
SubTrajectory& trajectory = impl->addTrajectory(std::move(t));
|
||||
trajectory.setEndState(to);
|
||||
impl->prevEnds()->add(std::move(from), NULL, &trajectory);
|
||||
@ -460,7 +458,6 @@ Generator::Generator(const std::string &name)
|
||||
|
||||
void Generator::spawn(InterfaceState&& state, SubTrajectory&& t)
|
||||
{
|
||||
std::cout << "spawning state forwards and backwards" << std::endl;
|
||||
assert(state.incomingTrajectories().empty() &&
|
||||
state.outgoingTrajectories().empty());
|
||||
assert(!t.trajectory());
|
||||
|
||||
@ -154,7 +154,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||
|
||||
const double achieved_distance= achieved_fraction*(link_pose.translation()-target_point).norm();
|
||||
|
||||
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
||||
ROS_INFO_NAMED(this->name().c_str(), "achieved %f of cartesian motion", achieved_distance);
|
||||
|
||||
succeeded= achieved_distance >= props.get<double>("min_distance");
|
||||
}
|
||||
@ -176,7 +176,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
||||
1.5, /* jump threshold */
|
||||
is_valid);
|
||||
|
||||
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
||||
ROS_INFO_NAMED(this->name().c_str(), "achieved %f of cartesian motion", achieved_distance);
|
||||
|
||||
succeeded= achieved_distance >= props.get<double>("min_distance");
|
||||
}
|
||||
@ -248,7 +248,7 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
||||
1.5, /* jump threshold */
|
||||
is_valid);
|
||||
|
||||
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
||||
ROS_INFO_NAMED(this->name().c_str(), "achieved %f of cartesian motion", achieved_distance);
|
||||
|
||||
bool succeeded= achieved_distance >= props.get<double>("min_distance");
|
||||
|
||||
|
||||
@ -58,7 +58,7 @@ Task::Task(const std::string& id, ContainerBase::pointer &&container)
|
||||
initModel();
|
||||
|
||||
// monitor state on commandline
|
||||
addTaskCallback(&printState);
|
||||
//addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
|
||||
// enable introspection by default
|
||||
enableIntrospection(true);
|
||||
}
|
||||
@ -232,6 +232,7 @@ bool Task::plan()
|
||||
} else
|
||||
break;
|
||||
}
|
||||
printState();
|
||||
return numSolutions() > 0;
|
||||
}
|
||||
|
||||
@ -285,12 +286,13 @@ std::string Task::id() const
|
||||
return id_;
|
||||
}
|
||||
|
||||
void Task::printState(const Task &t){
|
||||
ContainerBase::StageCallback processor = [](const Stage& stage, int depth) -> bool {
|
||||
std::cout << std::string(2*depth, ' ') << stage << std::endl;
|
||||
void Task::printState(std::ostream& os) const
|
||||
{
|
||||
ContainerBase::StageCallback processor = [&os](const Stage& stage, int depth) -> bool {
|
||||
os << std::string(2*depth, ' ') << stage << std::endl;
|
||||
return true;
|
||||
};
|
||||
t.stages()->traverseRecursively(processor);
|
||||
stages()->traverseRecursively(processor);
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -47,7 +47,5 @@ int main(int argc, char** argv){
|
||||
|
||||
t.plan();
|
||||
|
||||
Task::printState(t);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user