diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 26140060..d0ad411e 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -66,7 +66,7 @@ struct LinkMotion { LinkMotion(std::string link_name) : link_name(link_name) {} - double operator()(const SubTrajectory&, std::string&); + double operator()(const SubTrajectory&, std::string&) const; std::string link_name; }; @@ -85,7 +85,7 @@ struct Clearance Interface::Direction interface = Interface::NONE) : with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {} - double operator()(const SubTrajectory& s, std::string& comment); + double operator()(const SubTrajectory& s, std::string& comment) const; bool with_world; bool cumulative; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 12cf4d55..69ce13f2 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -64,7 +64,7 @@ double TrajectoryDuration(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -87,7 +87,7 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { return distance; } -double Clearance::operator()(const SubTrajectory& s, std::string& comment) { +double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; collision_detection::DistanceRequest request;