mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge ROS1 fixes
This commit is contained in:
commit
3b438f9b1c
@ -41,6 +41,7 @@
|
|||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/trajectory_processing/time_parameterization.h>
|
#include <moveit/trajectory_processing/time_parameterization.h>
|
||||||
|
#include <moveit/kinematics_base/kinematics_base.h>
|
||||||
#include <moveit/robot_state/cartesian_interpolator.h>
|
#include <moveit/robot_state/cartesian_interpolator.h>
|
||||||
|
|
||||||
using namespace trajectory_processing;
|
using namespace trajectory_processing;
|
||||||
@ -56,6 +57,8 @@ CartesianPath::CartesianPath() {
|
|||||||
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
|
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
|
||||||
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
|
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
|
||||||
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
|
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
|
||||||
|
p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
|
||||||
|
"KinematicsQueryOptions to pass to CartesianInterpolator");
|
||||||
}
|
}
|
||||||
|
|
||||||
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
|
||||||
@ -96,7 +99,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
|
|||||||
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
|
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
|
||||||
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
|
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
|
||||||
moveit::core::MaxEEFStep(props.get<double>("step_size")),
|
moveit::core::MaxEEFStep(props.get<double>("step_size")),
|
||||||
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid);
|
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
|
||||||
|
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"));
|
||||||
|
|
||||||
assert(!trajectory.empty()); // there should be at least the start state
|
assert(!trajectory.empty()); // there should be at least the start state
|
||||||
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
|
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
|
||||||
|
|||||||
@ -79,10 +79,12 @@ TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
|
|||||||
TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
|
TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
|
||||||
this->WrapperBasePrivate::operator=(std::move(other));
|
this->WrapperBasePrivate::operator=(std::move(other));
|
||||||
ns_ = std::move(other.ns_);
|
ns_ = std::move(other.ns_);
|
||||||
introspection_ = std::move(other.introspection_);
|
|
||||||
robot_model_ = std::move(other.robot_model_);
|
robot_model_ = std::move(other.robot_model_);
|
||||||
robot_model_loader_ = std::move(other.robot_model_loader_);
|
robot_model_loader_ = std::move(other.robot_model_loader_);
|
||||||
task_cbs_ = std::move(other.task_cbs_);
|
task_cbs_ = std::move(other.task_cbs_);
|
||||||
|
// Ensure same introspection status, but keep the existing introspection instance,
|
||||||
|
// which stores this task pointer and includes it in its task_id_
|
||||||
|
static_cast<Task*>(me_)->enableIntrospection(static_cast<bool>(other.introspection_));
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -211,17 +213,17 @@ void Task::init() {
|
|||||||
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
|
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
|
||||||
|
|
||||||
// provide introspection instance to all stages
|
// provide introspection instance to all stages
|
||||||
impl->setIntrospection(impl->introspection_.get());
|
auto* introspection = impl->introspection_.get();
|
||||||
impl->traverseStages(
|
impl->traverseStages(
|
||||||
[impl](Stage& stage, int /*depth*/) {
|
[introspection](Stage& stage, int /*depth*/) {
|
||||||
stage.pimpl()->setIntrospection(impl->introspection_.get());
|
stage.pimpl()->setIntrospection(introspection);
|
||||||
return true;
|
return true;
|
||||||
},
|
},
|
||||||
1, UINT_MAX);
|
1, UINT_MAX);
|
||||||
|
|
||||||
// first time publish task
|
// first time publish task
|
||||||
if (impl->introspection_)
|
if (introspection)
|
||||||
impl->introspection_->publishTaskDescription();
|
introspection->publishTaskDescription();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Task::canCompute() const {
|
bool Task::canCompute() const {
|
||||||
|
|||||||
@ -1,4 +1,5 @@
|
|||||||
#include "models.h"
|
#include "models.h"
|
||||||
|
#include "stage_mockups.h"
|
||||||
|
|
||||||
#include <moveit/task_constructor/task.h>
|
#include <moveit/task_constructor/task.h>
|
||||||
#include <moveit/task_constructor/stages/move_to.h>
|
#include <moveit/task_constructor/stages/move_to.h>
|
||||||
@ -156,6 +157,33 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
|
|||||||
EXPECT_ONE_SOLUTION;
|
EXPECT_ONE_SOLUTION;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// https://github.com/ros-planning/moveit_task_constructor/pull/371
|
||||||
|
TEST(Task, taskMoveConstructor) {
|
||||||
|
auto create_task = [] {
|
||||||
|
moveit::core::RobotModelConstPtr robot_model = getModel();
|
||||||
|
Task t("first");
|
||||||
|
t.setRobotModel(robot_model);
|
||||||
|
auto ref = new stages::FixedState("fixed");
|
||||||
|
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
|
||||||
|
ref->setState(scene);
|
||||||
|
|
||||||
|
t.add(Stage::pointer(ref));
|
||||||
|
t.add(std::make_unique<ConnectMockup>());
|
||||||
|
t.add(std::make_unique<MonitoringGeneratorMockup>(ref));
|
||||||
|
return t;
|
||||||
|
};
|
||||||
|
|
||||||
|
Task t;
|
||||||
|
t = create_task();
|
||||||
|
|
||||||
|
try {
|
||||||
|
t.init();
|
||||||
|
EXPECT_TRUE(t.plan(1));
|
||||||
|
} catch (const InitStageException& e) {
|
||||||
|
ADD_FAILURE() << "InitStageException:" << std::endl << e << t;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
int main(int argc, char** argv) {
|
int main(int argc, char** argv) {
|
||||||
testing::InitGoogleTest(&argc, argv);
|
testing::InitGoogleTest(&argc, argv);
|
||||||
rclcpp::init(argc, argv);
|
rclcpp::init(argc, argv);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user