refactor Forward -> PassThrough

based on review feedback
This commit is contained in:
v4hn 2020-09-12 16:07:09 +02:00
parent 64e2440c53
commit cdc7cb04fc
4 changed files with 20 additions and 20 deletions

View File

@ -49,10 +49,10 @@ namespace stages {
* Useful to set a custom CostTransform via Stage::setCostTerm
* to change a solution's cost without loosing the original value
*/
class Forward : public WrapperBase
class PassThrough : public WrapperBase
{
public:
Forward(const std::string& name = "Forward", Stage::pointer&& child = Stage::pointer());
PassThrough(const std::string& name = "PassThrough", Stage::pointer&& child = Stage::pointer());
void onNewSolution(const SolutionBase& s) override;
};

View File

@ -5,11 +5,11 @@ add_library(${PROJECT_NAME}_stages
${PROJECT_INCLUDE}/stages/current_state.h
${PROJECT_INCLUDE}/stages/fixed_state.h
${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h
${PROJECT_INCLUDE}/stages/forward.h
${PROJECT_INCLUDE}/stages/generate_pose.h
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
${PROJECT_INCLUDE}/stages/generate_place_pose.h
${PROJECT_INCLUDE}/stages/compute_ik.h
${PROJECT_INCLUDE}/stages/passthrough.h
${PROJECT_INCLUDE}/stages/predicate_filter.h
${PROJECT_INCLUDE}/stages/connect.h
@ -25,11 +25,11 @@ add_library(${PROJECT_NAME}_stages
current_state.cpp
fixed_state.cpp
fixed_cartesian_poses.cpp
forward.cpp
generate_pose.cpp
generate_grasp_pose.cpp
generate_place_pose.cpp
compute_ik.cpp
passthrough.cpp
predicate_filter.cpp
connect.cpp

View File

@ -33,7 +33,7 @@
*********************************************************************/
/* Author: Michael 'v4hn' Goerner */
#include <moveit/task_constructor/stages/forward.h>
#include <moveit/task_constructor/stages/passthrough.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
@ -54,9 +54,9 @@ namespace moveit {
namespace task_constructor {
namespace stages {
Forward::Forward(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {}
PassThrough::PassThrough(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {}
void Forward::onNewSolution(const SolutionBase& s) {
void PassThrough::onNewSolution(const SolutionBase& s) {
this->liftSolution(s);
}

View File

@ -3,7 +3,7 @@
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/stages/forward.h>
#include <moveit/task_constructor/stages/passthrough.h>
#include <moveit/planning_scene/planning_scene.h>
@ -238,10 +238,10 @@ TEST(CostTerm, StageTypes) {
EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator";
}
TEST(CostTerm, ForwardUsesCost) {
TEST(CostTerm, PassThroughUsesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::Forward> container(robot);
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant_stage{ 84.0 };
@ -250,32 +250,32 @@ TEST(CostTerm, ForwardUsesCost) {
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward cost";
EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "PassThrough forwards children's cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals PassThrough cost";
}
TEST(CostTerm, ForwardOverwritesCost) {
TEST(CostTerm, PassThroughOverwritesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::Forward> container(robot);
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant_stage{ 84.0 };
stage->setCostTerm(constant_stage);
cost::Constant constant_forward{ 72.0 };
container.setCostTerm(constant_forward);
cost::Constant constant_passthrough{ 72.0 };
container.setCostTerm(constant_passthrough);
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_forward.cost) << "Forward can apply custom cost";
EXPECT_EQ(wrapped.cost(), constant_passthrough.cost) << "PassThrough can apply custom cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected";
}
TEST(CostTerm, ForwardCanModifyCost) {
TEST(CostTerm, PassThroughCanModifyCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::Forward> container(robot);
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant{ 8.0 };
@ -284,7 +284,7 @@ TEST(CostTerm, ForwardCanModifyCost) {
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "Forward can compute cost based on child";
EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "PassThrough can compute cost based on child";
EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost) << "child's cost is not affected";
}