replace std::cout with ros console

This commit is contained in:
Robert Haschke 2018-02-16 15:12:54 +01:00
parent 8184156575
commit 2ea463fff4
7 changed files with 13 additions and 17 deletions

View File

@ -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;

View File

@ -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;
}

View File

@ -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);

View File

@ -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());

View File

@ -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");

View File

@ -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);
}
} }

View File

@ -47,7 +47,5 @@ int main(int argc, char** argv){
t.plan();
Task::printState(t);
return 0;
}