mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
refactor Forward -> PassThrough
based on review feedback
This commit is contained in:
parent
64e2440c53
commit
cdc7cb04fc
@ -49,10 +49,10 @@ namespace stages {
|
|||||||
* Useful to set a custom CostTransform via Stage::setCostTerm
|
* Useful to set a custom CostTransform via Stage::setCostTerm
|
||||||
* to change a solution's cost without loosing the original value
|
* to change a solution's cost without loosing the original value
|
||||||
*/
|
*/
|
||||||
class Forward : public WrapperBase
|
class PassThrough : public WrapperBase
|
||||||
{
|
{
|
||||||
public:
|
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;
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
};
|
};
|
||||||
@ -5,11 +5,11 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
${PROJECT_INCLUDE}/stages/current_state.h
|
${PROJECT_INCLUDE}/stages/current_state.h
|
||||||
${PROJECT_INCLUDE}/stages/fixed_state.h
|
${PROJECT_INCLUDE}/stages/fixed_state.h
|
||||||
${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h
|
${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h
|
||||||
${PROJECT_INCLUDE}/stages/forward.h
|
|
||||||
${PROJECT_INCLUDE}/stages/generate_pose.h
|
${PROJECT_INCLUDE}/stages/generate_pose.h
|
||||||
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
|
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
|
||||||
${PROJECT_INCLUDE}/stages/generate_place_pose.h
|
${PROJECT_INCLUDE}/stages/generate_place_pose.h
|
||||||
${PROJECT_INCLUDE}/stages/compute_ik.h
|
${PROJECT_INCLUDE}/stages/compute_ik.h
|
||||||
|
${PROJECT_INCLUDE}/stages/passthrough.h
|
||||||
${PROJECT_INCLUDE}/stages/predicate_filter.h
|
${PROJECT_INCLUDE}/stages/predicate_filter.h
|
||||||
|
|
||||||
${PROJECT_INCLUDE}/stages/connect.h
|
${PROJECT_INCLUDE}/stages/connect.h
|
||||||
@ -25,11 +25,11 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
current_state.cpp
|
current_state.cpp
|
||||||
fixed_state.cpp
|
fixed_state.cpp
|
||||||
fixed_cartesian_poses.cpp
|
fixed_cartesian_poses.cpp
|
||||||
forward.cpp
|
|
||||||
generate_pose.cpp
|
generate_pose.cpp
|
||||||
generate_grasp_pose.cpp
|
generate_grasp_pose.cpp
|
||||||
generate_place_pose.cpp
|
generate_place_pose.cpp
|
||||||
compute_ik.cpp
|
compute_ik.cpp
|
||||||
|
passthrough.cpp
|
||||||
predicate_filter.cpp
|
predicate_filter.cpp
|
||||||
|
|
||||||
connect.cpp
|
connect.cpp
|
||||||
|
|||||||
@ -33,7 +33,7 @@
|
|||||||
*********************************************************************/
|
*********************************************************************/
|
||||||
/* Author: Michael 'v4hn' Goerner */
|
/* 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/storage.h>
|
||||||
#include <moveit/task_constructor/marker_tools.h>
|
#include <moveit/task_constructor/marker_tools.h>
|
||||||
|
|
||||||
@ -54,9 +54,9 @@ namespace moveit {
|
|||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
namespace stages {
|
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);
|
this->liftSolution(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -3,7 +3,7 @@
|
|||||||
#include <moveit/task_constructor/cost_terms.h>
|
#include <moveit/task_constructor/cost_terms.h>
|
||||||
#include <moveit/task_constructor/stage_p.h>
|
#include <moveit/task_constructor/stage_p.h>
|
||||||
#include <moveit/task_constructor/container_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>
|
#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";
|
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);
|
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::Forward> container(robot);
|
Standalone<stages::PassThrough> container(robot);
|
||||||
|
|
||||||
auto stage{ std::make_unique<BackwardMockup>() };
|
auto stage{ std::make_unique<BackwardMockup>() };
|
||||||
cost::Constant constant_stage{ 84.0 };
|
cost::Constant constant_stage{ 84.0 };
|
||||||
@ -250,32 +250,32 @@ TEST(CostTerm, ForwardUsesCost) {
|
|||||||
container.computeWithStages({ std::move(stage) });
|
container.computeWithStages({ std::move(stage) });
|
||||||
|
|
||||||
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
|
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
|
||||||
EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost";
|
EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "PassThrough forwards children's cost";
|
||||||
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward 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);
|
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::Forward> container(robot);
|
Standalone<stages::PassThrough> container(robot);
|
||||||
|
|
||||||
auto stage{ std::make_unique<BackwardMockup>() };
|
auto stage{ std::make_unique<BackwardMockup>() };
|
||||||
cost::Constant constant_stage{ 84.0 };
|
cost::Constant constant_stage{ 84.0 };
|
||||||
stage->setCostTerm(constant_stage);
|
stage->setCostTerm(constant_stage);
|
||||||
|
|
||||||
cost::Constant constant_forward{ 72.0 };
|
cost::Constant constant_passthrough{ 72.0 };
|
||||||
container.setCostTerm(constant_forward);
|
container.setCostTerm(constant_passthrough);
|
||||||
|
|
||||||
container.computeWithStages({ std::move(stage) });
|
container.computeWithStages({ std::move(stage) });
|
||||||
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
|
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";
|
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);
|
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::Forward> container(robot);
|
Standalone<stages::PassThrough> container(robot);
|
||||||
|
|
||||||
auto stage{ std::make_unique<BackwardMockup>() };
|
auto stage{ std::make_unique<BackwardMockup>() };
|
||||||
cost::Constant constant{ 8.0 };
|
cost::Constant constant{ 8.0 };
|
||||||
@ -284,7 +284,7 @@ TEST(CostTerm, ForwardCanModifyCost) {
|
|||||||
|
|
||||||
container.computeWithStages({ std::move(stage) });
|
container.computeWithStages({ std::move(stage) });
|
||||||
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
|
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";
|
EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost) << "child's cost is not affected";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user