mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branch master into ros2
This commit is contained in:
commit
9924dfe25f
@ -1,5 +1,7 @@
|
||||
---
|
||||
Checks: 'performance-*,
|
||||
-performance-noexcept-move-constructor,
|
||||
-clang-analyzer-core.CallAndMessage,
|
||||
llvm-namespace-comment,
|
||||
modernize-redundant-void-arg,
|
||||
modernize-use-nullptr,
|
||||
@ -12,7 +14,6 @@ Checks: 'performance-*,
|
||||
readability-redundant-string-cstr,
|
||||
readability-simplify-boolean-expr,
|
||||
readability-container-size-empty,
|
||||
readability-identifier-naming,
|
||||
'
|
||||
HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h'
|
||||
AnalyzeTemporaryDtors: false
|
||||
@ -25,14 +26,14 @@ CheckOptions:
|
||||
value: '2'
|
||||
# type names
|
||||
- key: readability-identifier-naming.ClassCase
|
||||
value: aNy_CasE # CamelCase
|
||||
value: CamelCase
|
||||
- key: readability-identifier-naming.EnumCase
|
||||
value: CamelCase
|
||||
- key: readability-identifier-naming.UnionCase
|
||||
value: CamelCase
|
||||
# method names
|
||||
- key: readability-identifier-naming.MethodCase
|
||||
value: aNy_CasE # camelBack
|
||||
value: camelBack
|
||||
# variable names
|
||||
- key: readability-identifier-naming.VariableCase
|
||||
value: lower_case
|
||||
|
||||
12
.github/workflows/ci.yaml
vendored
12
.github/workflows/ci.yaml
vendored
@ -24,7 +24,7 @@ jobs:
|
||||
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith
|
||||
- IMAGE: rolling-source
|
||||
CXX: clang++
|
||||
CLANG_TIDY: true
|
||||
CLANG_TIDY: pedantic
|
||||
CXXFLAGS: >-
|
||||
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith
|
||||
-Wno-deprecated-copy
|
||||
@ -56,16 +56,18 @@ jobs:
|
||||
name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v2
|
||||
- uses: actions/checkout@v3
|
||||
|
||||
- name: Cache ccache
|
||||
uses: pat-s/always-upload-cache@v2.1.5
|
||||
uses: rhaschke/cache@main
|
||||
with:
|
||||
path: ${{ env.CCACHE_DIR }}
|
||||
key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
|
||||
restore-keys: |
|
||||
ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
|
||||
ccache-${{ env.CACHE_PREFIX }}
|
||||
env:
|
||||
GHA_CACHE_SAVE: always
|
||||
|
||||
- id: ici
|
||||
name: Run industrial_ci
|
||||
@ -73,7 +75,7 @@ jobs:
|
||||
env: ${{ matrix.env }}
|
||||
|
||||
- name: Upload test artifacts (on failure)
|
||||
uses: actions/upload-artifact@v2
|
||||
uses: actions/upload-artifact@v3
|
||||
if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
|
||||
with:
|
||||
name: test-results-${{ matrix.env.IMAGE }}
|
||||
@ -86,7 +88,7 @@ jobs:
|
||||
workdir: ${{ env.BASEDIR }}/target_ws
|
||||
ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
|
||||
- name: Upload codecov report
|
||||
uses: codecov/codecov-action@v2
|
||||
uses: codecov/codecov-action@v3
|
||||
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
|
||||
with:
|
||||
files: ${{ env.BASEDIR }}/target_ws/coverage.info
|
||||
|
||||
@ -44,10 +44,3 @@
|
||||
#define MOVEIT_VERSION_GE(major, minor, patch) \
|
||||
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
|
||||
major * 1'000'000 + minor * 1'000 + patch)
|
||||
|
||||
#if !MOVEIT_VERSION_GE(3, 0, 0)
|
||||
// the pointers are not yet available
|
||||
namespace trajectory_processing {
|
||||
MOVEIT_CLASS_FORWARD(TimeParameterization);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -66,8 +66,8 @@ public:
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
};
|
||||
} // namespace solvers
|
||||
|
||||
@ -63,8 +63,8 @@ public:
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
};
|
||||
} // namespace solvers
|
||||
|
||||
@ -90,8 +90,8 @@ public:
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
|
||||
|
||||
protected:
|
||||
|
||||
@ -84,9 +84,10 @@ public:
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
|
||||
|
||||
/// plan trajectory from current robot state to Cartesian target
|
||||
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
|
||||
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) = 0;
|
||||
};
|
||||
|
||||
@ -48,7 +48,7 @@ namespace stages {
|
||||
class FixedState : public Generator
|
||||
{
|
||||
public:
|
||||
FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr);
|
||||
FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr scene = nullptr);
|
||||
void setState(const planning_scene::PlanningScenePtr& scene);
|
||||
|
||||
void reset() override;
|
||||
|
||||
@ -249,7 +249,7 @@ private:
|
||||
|
||||
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
|
||||
std::ostream& operator<<(std::ostream& os, const Interface& interface);
|
||||
std::ostream& operator<<(std::ostream& os, Interface::Direction);
|
||||
std::ostream& operator<<(std::ostream& os, Interface::Direction dir);
|
||||
|
||||
/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
|
||||
template <typename T>
|
||||
|
||||
@ -140,9 +140,6 @@ private:
|
||||
Int i;
|
||||
};
|
||||
|
||||
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
||||
std::string frame);
|
||||
|
||||
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
|
||||
const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
|
||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/merge.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
|
||||
|
||||
@ -1051,7 +1050,7 @@ void FallbacksPrivateConnect::compute() {
|
||||
|
||||
void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
|
||||
// expect failure to be reported from active child
|
||||
assert(active_ != children().end() && active_->get() == &child);
|
||||
assert(active_ != children().end() && active_->get() == &child); (void)child;
|
||||
// ... thus we can use std::next(active_) to find the next child
|
||||
auto next = std::next(active_);
|
||||
|
||||
|
||||
@ -36,7 +36,6 @@
|
||||
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/collision_detection/collision_common.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
|
||||
@ -37,8 +37,6 @@
|
||||
*/
|
||||
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/trajectory_processing/time_parameterization.h>
|
||||
#include <moveit/kinematics_base/kinematics_base.h>
|
||||
@ -76,11 +74,13 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
}
|
||||
|
||||
// reach pose of forward kinematics
|
||||
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints);
|
||||
return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg,
|
||||
timeout, result, path_constraints);
|
||||
}
|
||||
|
||||
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
|
||||
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
@ -103,7 +103,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
|
||||
moveit::core::MaxEEFStep(props.get<double>("step_size")),
|
||||
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid,
|
||||
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"),
|
||||
props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"));
|
||||
props.get<kinematics::KinematicsBase::IKCostFn>("kinematics_cost_fn"), offset);
|
||||
|
||||
assert(!trajectory.empty()); // there should be at least the start state
|
||||
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
*/
|
||||
|
||||
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/trajectory_processing/time_parameterization.h>
|
||||
|
||||
@ -102,9 +101,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
||||
}
|
||||
|
||||
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
|
||||
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||
const auto start_time = std::chrono::steady_clock::now();
|
||||
|
||||
@ -120,7 +119,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
||||
return to->isStateValid(*robot_state, constraints, jmg->getName());
|
||||
} };
|
||||
|
||||
if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) {
|
||||
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid)) {
|
||||
// TODO(v4hn): planners need a way to add feedback to failing plans
|
||||
// in case of an invalid solution feedback should include unwanted collisions or violated constraints
|
||||
RCLCPP_WARN(LOGGER, "IK failed for pose target");
|
||||
|
||||
@ -185,8 +185,9 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
|
||||
}
|
||||
|
||||
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
|
||||
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg,
|
||||
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
|
||||
const moveit::core::JointModelGroup* jmg, double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::msg::Constraints& path_constraints) {
|
||||
const auto& props = properties();
|
||||
moveit_msgs::msg::MotionPlanRequest req;
|
||||
@ -194,7 +195,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
|
||||
|
||||
geometry_msgs::msg::PoseStamped target;
|
||||
target.header.frame_id = from->getPlanningFrame();
|
||||
target.pose = tf2::toMsg(target_eigen);
|
||||
target.pose = tf2::toMsg(target_eigen * offset.inverse());
|
||||
|
||||
req.goal_constraints.resize(1);
|
||||
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
*/
|
||||
|
||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
|
||||
|
||||
using namespace trajectory_processing;
|
||||
|
||||
@ -38,7 +38,6 @@
|
||||
#include <moveit/task_constructor/stage_p.h>
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
#include <moveit/task_constructor/introspection.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -126,8 +125,8 @@ StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
|
||||
prev_ends_ = std::move(other.prev_ends_);
|
||||
next_starts_ = std::move(other.next_starts_);
|
||||
|
||||
parent_ = std::move(other.parent_);
|
||||
it_ = std::move(other.it_);
|
||||
parent_ = other.parent_;
|
||||
it_ = other.it_;
|
||||
other.unparent();
|
||||
|
||||
return *this;
|
||||
|
||||
@ -37,7 +37,6 @@
|
||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/marker_tools.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
||||
@ -90,7 +89,15 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string&
|
||||
}
|
||||
|
||||
// found IK solutions
|
||||
using IKSolutions = std::vector<std::vector<double>>;
|
||||
|
||||
struct IKSolution
|
||||
{
|
||||
std::vector<double> joint_positions;
|
||||
bool feasible;
|
||||
collision_detection::Contact contact;
|
||||
};
|
||||
|
||||
using IKSolutions = std::vector<IKSolution>;
|
||||
|
||||
namespace {
|
||||
|
||||
@ -358,14 +365,23 @@ void ComputeIK::compute() {
|
||||
&ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg,
|
||||
const double* joint_positions) {
|
||||
for (const auto& sol : ik_solutions) {
|
||||
if (jmg->distance(joint_positions, sol.data()) < min_solution_distance)
|
||||
if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance)
|
||||
return false; // too close to already found solution
|
||||
}
|
||||
state->setJointGroupPositions(jmg, joint_positions);
|
||||
ik_solutions.emplace_back();
|
||||
state->copyJointGroupPositions(jmg, ik_solutions.back());
|
||||
|
||||
return ignore_collisions || !scene->isStateColliding(*state, jmg->getName());
|
||||
auto& solution{ ik_solutions.back() };
|
||||
state->copyJointGroupPositions(jmg, solution.joint_positions);
|
||||
collision_detection::CollisionRequest req;
|
||||
collision_detection::CollisionResult res;
|
||||
req.contacts = true;
|
||||
req.max_contacts = 1;
|
||||
scene->checkCollision(req, res, *state);
|
||||
solution.feasible = ignore_collisions || !res.collision;
|
||||
if (!res.contacts.empty()) {
|
||||
solution.contact = res.contacts.begin()->second.front();
|
||||
}
|
||||
return solution.feasible;
|
||||
};
|
||||
|
||||
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
|
||||
@ -393,15 +409,18 @@ void ComputeIK::compute() {
|
||||
solution.setComment(s.comment());
|
||||
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
|
||||
|
||||
if (succeeded && i + 1 == ik_solutions.size())
|
||||
if (ik_solutions[i].feasible)
|
||||
// compute cost as distance to compare_pose
|
||||
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
|
||||
else // found an IK solution, but this was not valid
|
||||
solution.markAsFailure();
|
||||
|
||||
solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data()));
|
||||
else { // found an IK solution, but this was not valid
|
||||
std::stringstream ss;
|
||||
ss << "Collision between '" << ik_solutions[i].contact.body_name_1 << "' and '"
|
||||
<< ik_solutions[i].contact.body_name_2 << "'";
|
||||
solution.markAsFailure(ss.str());
|
||||
}
|
||||
// set scene's robot state
|
||||
moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
|
||||
solution_state.setJointGroupPositions(jmg, ik_solutions[i].data());
|
||||
solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data());
|
||||
solution_state.update();
|
||||
|
||||
InterfaceState state(solution_scene);
|
||||
|
||||
@ -38,7 +38,6 @@
|
||||
|
||||
#include <moveit/task_constructor/stages/connect.h>
|
||||
#include <moveit/task_constructor/merge.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -38,7 +38,6 @@
|
||||
|
||||
#include <moveit/task_constructor/stages/fix_collision_objects.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
|
||||
@ -44,7 +44,7 @@ namespace task_constructor {
|
||||
namespace stages {
|
||||
|
||||
FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene)
|
||||
: Generator(name), scene_(scene) {
|
||||
: Generator(name), scene_(std::move(scene)) {
|
||||
properties().declare("ignore_collisions", false);
|
||||
setCostTerm(std::make_unique<cost::Constant>(0.0));
|
||||
}
|
||||
|
||||
@ -110,8 +110,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
|
||||
return false;
|
||||
}
|
||||
|
||||
// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance
|
||||
static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers, Interface::Direction dir, bool success,
|
||||
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose,
|
||||
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose,
|
||||
const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
|
||||
double linear_norm = linear.norm();
|
||||
|
||||
@ -120,7 +121,7 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
|
||||
auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
|
||||
|
||||
// link position before planning; reached link position after planning; target link position
|
||||
Eigen::Vector3d pos_link = link_pose.translation();
|
||||
Eigen::Vector3d pos_start = start_pose.translation();
|
||||
Eigen::Vector3d pos_reached = reached_pose.translation();
|
||||
Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0);
|
||||
|
||||
@ -130,7 +131,7 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
|
||||
if (dir == Interface::FORWARD) {
|
||||
if (success) {
|
||||
// valid part: green arrow
|
||||
rviz_marker_tools::makeArrow(m, pos_link, pos_reached, 0.1 * linear_norm);
|
||||
rviz_marker_tools::makeArrow(m, pos_start, pos_reached, 0.1 * linear_norm);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
markers.push_back(m);
|
||||
} else {
|
||||
@ -145,14 +146,14 @@ static void visualizePlan(std::deque<visualization_msgs::msg::Marker>& markers,
|
||||
rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
// position half-way between pos_link and pos_reached
|
||||
m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) });
|
||||
m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_start + pos_reached) });
|
||||
m.pose.orientation = tf2::toMsg(quat_cylinder);
|
||||
markers.push_back(m);
|
||||
}
|
||||
} else {
|
||||
// valid part: green arrow
|
||||
// head length according to above comment
|
||||
rviz_marker_tools::makeArrow(m, pos_reached, pos_link, 0.1 * linear_norm, 0.23 * linear_norm);
|
||||
rviz_marker_tools::makeArrow(m, pos_reached, pos_start, 0.1 * linear_norm, 0.23 * linear_norm);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
markers.push_back(m);
|
||||
if (!success) {
|
||||
@ -209,10 +210,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
Eigen::Vector3d linear; // linear translation
|
||||
Eigen::Vector3d angular; // angular rotation
|
||||
double linear_norm = 0.0, angular_norm = 0.0;
|
||||
|
||||
Eigen::Isometry3d target_eigen;
|
||||
Eigen::Isometry3d link_pose =
|
||||
scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
|
||||
|
||||
try { // try to extract Twist
|
||||
const geometry_msgs::msg::TwistStamped& target = boost::any_cast<geometry_msgs::msg::TwistStamped>(direction);
|
||||
@ -246,13 +244,13 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
angular *= -1.0;
|
||||
}
|
||||
|
||||
// compute absolute transform for link
|
||||
// compute target transform for ik_frame applying motion transform of twist
|
||||
// linear+angular are expressed w.r.t. model frame and thus we need left-multiplication
|
||||
linear = frame_pose.linear() * linear;
|
||||
angular = frame_pose.linear() * angular;
|
||||
target_eigen = ik_pose_world;
|
||||
target_eigen.linear() =
|
||||
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular);
|
||||
target_eigen.translation() += linear;
|
||||
auto R = Eigen::AngleAxisd(angular_norm, angular);
|
||||
auto p = ik_pose_world.translation();
|
||||
target_eigen = Eigen::Translation3d(linear + p - R * p) * (R * ik_pose_world);
|
||||
goto COMPUTE;
|
||||
} catch (const boost::bad_any_cast&) { /* continue with Vector */
|
||||
}
|
||||
@ -274,31 +272,31 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
if (dir == Interface::BACKWARD)
|
||||
linear *= -1.0;
|
||||
|
||||
// compute absolute transform for link
|
||||
// compute target transform for ik_frame applying delta transform of twist
|
||||
linear = frame_pose.linear() * linear;
|
||||
target_eigen = ik_pose_world;
|
||||
target_eigen.translation() += linear;
|
||||
target_eigen = Eigen::Translation3d(linear) * ik_pose_world;
|
||||
} catch (const boost::bad_any_cast&) {
|
||||
solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
|
||||
return false;
|
||||
}
|
||||
|
||||
COMPUTE:
|
||||
// transform target pose such that ik frame will reach there if link does
|
||||
target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
|
||||
// offset from link to ik_frame
|
||||
const Eigen::Isometry3d& offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
|
||||
|
||||
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success =
|
||||
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
|
||||
moveit::core::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
|
||||
|
||||
double distance = 0.0;
|
||||
if (use_rotation_distance) {
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
|
||||
distance = rotation.angle();
|
||||
} else
|
||||
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
||||
distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
|
||||
|
||||
// min_distance reached?
|
||||
if (min_distance > 0.0) {
|
||||
@ -316,8 +314,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
// visualize plan
|
||||
auto ns = props.get<std::string>("marker_ns");
|
||||
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance
|
||||
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), link_pose, reached_pose, linear,
|
||||
distance);
|
||||
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
|
||||
linear, distance);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -41,7 +41,6 @@
|
||||
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/task_constructor/cost_terms.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/task_constructor/utils.h>
|
||||
|
||||
#include <rviz_marker_tools/marker_creation.h>
|
||||
@ -237,11 +236,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
||||
add_frame(target, "target frame");
|
||||
add_frame(ik_pose_world, "ik frame");
|
||||
|
||||
// transform target pose such that ik frame will reach there if link does
|
||||
target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
|
||||
// offset from link to ik_frame
|
||||
Eigen::Isometry3d offset = scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
|
||||
|
||||
// plan to Cartesian target
|
||||
success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||
success = planner_->plan(state.scene(), *link, offset, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||
}
|
||||
|
||||
// store result
|
||||
|
||||
@ -249,7 +249,7 @@ moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
|
||||
while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
|
||||
if (impl->preempt_requested_)
|
||||
return success_or(moveit::core::MoveItErrorCode::PREEMPTED);
|
||||
if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() > available_time)
|
||||
if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() >= available_time)
|
||||
return success_or(moveit::core::MoveItErrorCode::TIMED_OUT);
|
||||
compute();
|
||||
for (const auto& cb : impl->task_cbs_)
|
||||
|
||||
@ -44,7 +44,6 @@
|
||||
#include <moveit/robot_state/robot_state.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <moveit/task_constructor/storage.h>
|
||||
|
||||
@ -52,11 +51,6 @@ namespace moveit {
|
||||
namespace task_constructor {
|
||||
namespace utils {
|
||||
|
||||
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
||||
std::string frame) {
|
||||
return state.getRigidlyConnectedParentLinkModel(frame);
|
||||
}
|
||||
|
||||
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
|
||||
const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
|
||||
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) {
|
||||
@ -79,23 +73,27 @@ bool getRobotTipForFrame(const Property& property, const planning_scene::Plannin
|
||||
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
|
||||
} else {
|
||||
auto ik_pose_msg = boost::any_cast<geometry_msgs::msg::PoseStamped>(property.value());
|
||||
if (ik_pose_msg.header.frame_id.empty()) {
|
||||
if (!(robot_link = get_tip())) {
|
||||
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
|
||||
return false;
|
||||
}
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame;
|
||||
} else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) {
|
||||
robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id);
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame;
|
||||
} else {
|
||||
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
|
||||
|
||||
robot_link = nullptr;
|
||||
bool found = false;
|
||||
auto ref_frame = scene.getCurrentState().getFrameInfo(ik_pose_msg.header.frame_id, robot_link, found);
|
||||
if (!found && !ik_pose_msg.header.frame_id.empty()) {
|
||||
std::stringstream ss;
|
||||
ss << "ik_frame specified in unknown frame '" << ik_pose_msg.header.frame_id << "'";
|
||||
solution.markAsFailure(ss.str());
|
||||
return false;
|
||||
}
|
||||
if (!robot_link)
|
||||
robot_link = get_tip();
|
||||
if (!robot_link) {
|
||||
solution.markAsFailure("ik_frame doesn't specify a link frame");
|
||||
return false;
|
||||
} else if (!found) { // use robot link's frame as reference by default
|
||||
ref_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
|
||||
}
|
||||
|
||||
tip_in_global_frame = ref_frame * tip_in_global_frame;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
@ -4,9 +4,9 @@
|
||||
#include <moveit/task_constructor/stages/move_relative.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist_stamped.hpp>
|
||||
@ -18,7 +18,7 @@ using namespace planning_scene;
|
||||
using namespace moveit::core;
|
||||
|
||||
constexpr double TAU{ 2 * M_PI };
|
||||
constexpr double EPS{ 1e-6 };
|
||||
constexpr double EPS{ 5e-5 };
|
||||
|
||||
// provide a basic test fixture that prepares a Task
|
||||
struct PandaMoveRelative : public testing::Test
|
||||
@ -68,10 +68,24 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string
|
||||
return aco;
|
||||
}
|
||||
|
||||
inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) {
|
||||
return scene->getFrameTransform(frame).translation();
|
||||
void expect_const_position(const SolutionBaseConstPtr& solution, const std::string& tip,
|
||||
const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity()) {
|
||||
const robot_trajectory::RobotTrajectory& t = *std::dynamic_pointer_cast<const SubTrajectory>(solution)->trajectory();
|
||||
const Eigen::Vector3d start_position = (t.getFirstWayPoint().getFrameTransform(tip) * offset).translation();
|
||||
for (size_t i = 0; i < t.getWayPointCount(); ++i) {
|
||||
const Eigen::Vector3d position = (t.getWayPoint(i).getFrameTransform(tip) * offset).translation();
|
||||
ASSERT_TRUE(start_position.isApprox(position, EPS))
|
||||
<< "Rotation must maintain position\n"
|
||||
<< i << ": " << start_position.transpose() << " != " << position.transpose();
|
||||
}
|
||||
}
|
||||
|
||||
#define EXPECT_CONST_POSITION(...) \
|
||||
{ \
|
||||
SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \
|
||||
expect_const_position(__VA_ARGS__); \
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveRelative, cartesianRotateEEF) {
|
||||
move->setDirection([] {
|
||||
geometry_msgs::msg::TwistStamped twist;
|
||||
@ -81,21 +95,28 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) {
|
||||
}());
|
||||
|
||||
ASSERT_TRUE(t.plan()) << "Failed to plan";
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName());
|
||||
}
|
||||
|
||||
const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() };
|
||||
const auto start_eef_position{ position(scene, tip_name) };
|
||||
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) };
|
||||
TEST_F(PandaMoveRelative, cartesianCircular) {
|
||||
const std::string tip = "panda_hand";
|
||||
auto offset = Eigen::Translation3d(0, 0, 0.1);
|
||||
move->setIKFrame(offset, tip);
|
||||
move->setDirection([] {
|
||||
geometry_msgs::msg::TwistStamped twist;
|
||||
twist.header.frame_id = "world";
|
||||
twist.twist.angular.x = TAU / 4.0;
|
||||
return twist;
|
||||
}());
|
||||
|
||||
EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS))
|
||||
<< "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n"
|
||||
<< start_eef_position << "\nvs\n"
|
||||
<< end_eef_position;
|
||||
ASSERT_TRUE(t.plan()) << "Failed to plan";
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset));
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
|
||||
const std::string ATTACHED_OBJECT{ "attached_object" };
|
||||
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
|
||||
move->setIKFrame(ATTACHED_OBJECT);
|
||||
const std::string attached_object{ "attached_object" };
|
||||
scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object));
|
||||
move->setIKFrame(attached_object);
|
||||
|
||||
move->setDirection([] {
|
||||
geometry_msgs::msg::TwistStamped twist;
|
||||
@ -104,15 +125,8 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
|
||||
return twist;
|
||||
}());
|
||||
|
||||
ASSERT_TRUE(t.plan());
|
||||
|
||||
const auto start_eef_position{ position(scene, ATTACHED_OBJECT) };
|
||||
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) };
|
||||
|
||||
EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS))
|
||||
<< "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n"
|
||||
<< start_eef_position << "\nvs\n"
|
||||
<< end_eef_position;
|
||||
ASSERT_TRUE(t.plan()) << "Failed to plan";
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), attached_object);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
|
||||
@ -5,7 +5,6 @@
|
||||
#include <moveit/task_constructor/stages/move_to.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
@ -78,7 +77,8 @@ TEST_F(PandaMoveTo, stateTarget) {
|
||||
EXPECT_ONE_SOLUTION;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) {
|
||||
geometry_msgs::msg::PoseStamped getFramePoseOfNamedState(RobotState state, const std::string& pose,
|
||||
const std::string& frame) {
|
||||
state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose);
|
||||
auto frame_eigen{ state.getFrameTransform(frame) };
|
||||
geometry_msgs::msg::PoseStamped p;
|
||||
@ -104,9 +104,9 @@ TEST_F(PandaMoveTo, poseTarget) {
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveTo, poseIKFrameLinkTarget) {
|
||||
const std::string IK_FRAME{ "panda_hand" };
|
||||
move_to->setIKFrame(IK_FRAME);
|
||||
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
|
||||
const std::string ik_frame{ "panda_hand" };
|
||||
move_to->setIKFrame(ik_frame);
|
||||
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame));
|
||||
EXPECT_ONE_SOLUTION;
|
||||
}
|
||||
|
||||
@ -123,10 +123,8 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string
|
||||
p.dimensions[p.SPHERE_RADIUS] = 0.01;
|
||||
return p;
|
||||
}());
|
||||
aco.object.primitive_poses.resize(1);
|
||||
aco.object.pose.position.z = 0.2;
|
||||
aco.object.pose.orientation.w = 1.0;
|
||||
// If we don't have this, we also don't have subframe support
|
||||
aco.object.subframe_names.resize(1, "subframe");
|
||||
aco.object.subframe_poses.resize(1, [] {
|
||||
geometry_msgs::msg::Pose p;
|
||||
@ -137,23 +135,22 @@ moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) {
|
||||
const std::string ATTACHED_OBJECT{ "attached_object" };
|
||||
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
|
||||
const std::string attached_object{ "attached_object" };
|
||||
scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object));
|
||||
|
||||
move_to->setIKFrame(ATTACHED_OBJECT);
|
||||
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT));
|
||||
move_to->setIKFrame(attached_object);
|
||||
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", attached_object));
|
||||
EXPECT_ONE_SOLUTION;
|
||||
}
|
||||
|
||||
// If we don't have this, we also don't have subframe support
|
||||
TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
|
||||
const std::string ATTACHED_OBJECT{ "attached_object" };
|
||||
const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" };
|
||||
const std::string attached_object{ "attached_object" };
|
||||
const std::string ik_frame{ attached_object + "/subframe" };
|
||||
|
||||
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
|
||||
scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object));
|
||||
|
||||
move_to->setIKFrame(IK_FRAME);
|
||||
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
|
||||
move_to->setIKFrame(ik_frame);
|
||||
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ik_frame));
|
||||
EXPECT_ONE_SOLUTION;
|
||||
}
|
||||
|
||||
|
||||
@ -21,7 +21,7 @@ struct StandaloneGeneratorMockup : public GeneratorMockup
|
||||
InterfacePtr next;
|
||||
|
||||
StandaloneGeneratorMockup(std::initializer_list<double>&& costs)
|
||||
: StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {}
|
||||
: StandaloneGeneratorMockup{ PredefinedCosts{ costs, true } } {}
|
||||
|
||||
StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true })
|
||||
: GeneratorMockup{ std::move(costs) } {
|
||||
|
||||
@ -3,8 +3,6 @@
|
||||
#include <moveit/robot_model/robot_model.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
|
||||
#include <moveit/task_constructor/moveit_compat.h>
|
||||
|
||||
#include <moveit/task_constructor/task.h>
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
|
||||
@ -149,7 +149,7 @@ void PropertyFactory::addRemainingProperties(rviz_common::properties::Property*
|
||||
}
|
||||
|
||||
#ifndef HAVE_YAML
|
||||
rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type,
|
||||
rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/,
|
||||
const std::string& description,
|
||||
const std::string& value,
|
||||
rviz_common::properties::Property* old) {
|
||||
|
||||
@ -176,7 +176,7 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) {
|
||||
tool_buttons_group = new QButtonGroup(panel);
|
||||
tool_buttons_group->setExclusive(true);
|
||||
button_show_stage_dock_widget->setEnabled(bool(getStageFactory()));
|
||||
button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages"));
|
||||
button_show_stage_dock_widget->setToolTip("Show available stages");
|
||||
property_root = new rviz_common::properties::Property("Global Settings");
|
||||
}
|
||||
|
||||
|
||||
@ -44,6 +44,7 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <initializer_list>
|
||||
#include <qcoreapplication.h>
|
||||
#include <qtimer.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
|
||||
@ -224,12 +225,19 @@ TEST_F(TaskListModelTest, deletion) {
|
||||
// process deleteLater() events
|
||||
QCoreApplication::sendPostedEvents(nullptr, QEvent::DeferredDelete);
|
||||
// as m is owned by model, m should be destroyed
|
||||
// EXPECT_EQ(num_deletes, 1); // TODO: event is not processed, missing event loop?
|
||||
EXPECT_EQ(num_deletes, 1);
|
||||
EXPECT_EQ(model.rowCount(), 0);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
rclcpp::init(argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
QCoreApplication app(argc, argv);
|
||||
// https://bugs.llvm.org/show_bug.cgi?id=40367
|
||||
// NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks)
|
||||
QTimer::singleShot(0, [&]() {
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
auto testResult = RUN_ALL_TESTS();
|
||||
app.exit(testResult);
|
||||
});
|
||||
return app.exec();
|
||||
}
|
||||
|
||||
@ -467,6 +467,7 @@ std::pair<QAbstractItemModel*, QModelIndex> FlatMergeProxyModel::getModel(const
|
||||
const QModelIndex& src_index = d_ptr->mapToSource(index, data);
|
||||
|
||||
Q_ASSERT(data);
|
||||
// NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker)
|
||||
return std::make_pair(data->model_, src_index);
|
||||
}
|
||||
|
||||
|
||||
@ -55,7 +55,7 @@ static QPixmap maskToColorAndAlpha(const QPixmap& mask, const QColor& color) {
|
||||
|
||||
using MaskAndColor = QPair<QPixmap, QColor>;
|
||||
using MasksAndColors = QList<MaskAndColor>;
|
||||
static MasksAndColors masksAndColors(const Icon& icon, int dpr) {
|
||||
static MasksAndColors masksAndColors(const Icon& icon, int /*dpr*/) {
|
||||
MasksAndColors result;
|
||||
for (const IconMaskAndColor& i : icon) {
|
||||
const QString& file_name = i.first;
|
||||
|
||||
@ -440,6 +440,7 @@ std::pair<QAbstractItemModel*, QModelIndex> TreeMergeProxyModel::getModel(const
|
||||
const QModelIndex& src_index = d_ptr->mapToSource(index, data);
|
||||
Q_ASSERT(data);
|
||||
|
||||
// NOLINTNEXTLINE(clang-analyzer-core.NonNullParamChecker)
|
||||
return std::make_pair(data->model_, src_index);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user