diff --git a/data/armarx_navigation/controller_config/PlatformTrajectory/default.json b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
index c9ffcb3fa92a8c0227daf32bf802b58b181f4a21..e69bb8299d15eab5f3de0eb5219308b88f9cd51c 100644
--- a/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
+++ b/data/armarx_navigation/controller_config/PlatformTrajectory/default.json
@@ -1,12 +1,12 @@
{
"params": {
"pidPos": {
- "Kp": 1,
+ "Kp": 0.1,
"Ki": 0,
"Kd": 0
},
"pidOri": {
- "Kp": 1,
+ "Kp": 0.1,
"Ki": 0,
"Kd": 0
},
diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
new file mode 100644
index 0000000000000000000000000000000000000000..0dcf777ba27257820a974dde220212696983348c
--- /dev/null
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -0,0 +1,115 @@
+{
+ "robot_footprint_radius": 0.5,
+ "planning_distance": 3,
+
+ "teb_config": {
+ "pse": {
+ "pse_costum_obstacle_penalties": true,
+ "pse_costum_obstacle_penalties_dynamic": true,
+ "weight_global_path": 0.3,
+ "lrk_use_alternative_approach": false,
+ "lrk_enable_collision_check": true,
+ "hybrid_homotopy_calculation": true,
+ "robot_diff_circumscribed_inscribed_radius": 0.0
+ },
+
+ "trajectory": {
+ "teb_autosize": true,
+ "dt_ref": 0.3,
+ "dt_hysteresis": 0.1,
+ "min_samples": 3,
+ "max_samples": 500,
+ "global_plan_overwrite_orientation": true,
+ "allow_init_with_backwards_motion": false,
+ "exact_arc_length": false,
+ "force_reinit_new_goal_dist": 1,
+ "force_reinit_new_goal_angular": 1.5707963267948966,
+ "feasibility_check_no_poses": 5,
+ "feasibility_check_lookahead_distance": -1,
+ "min_resolution_collision_check_angular": 3.141592653589793
+ },
+
+ "robot": {
+ "max_vel_x": 0.5,
+ "max_vel_x_backwards": 0.5,
+ "max_vel_y": 0.5,
+ "max_vel_trans": 0.5,
+ "max_vel_theta": 0.5,
+ "acc_lim_x": 0.5,
+ "acc_lim_y": 0.5,
+ "acc_lim_theta": 0.5,
+ "min_turning_radius": 0
+ },
+
+ "goal_tolerance": {
+ "xy_goal_tolerance": 0.2
+ },
+
+ "obstacles": {
+ "min_obstacle_dist": 0.3,
+ "inflation_dist": 0.0,
+ "dynamic_obstacle_inflation_dist": 0.6,
+ "include_dynamic_obstacles": true,
+ "obstacle_association_force_inclusion_factor": 1.5,
+ "obstacle_association_cutoff_factor": 5,
+ "obstacle_proximity_ratio_max_vel": 1,
+ "obstacle_proximity_lower_bound": 0,
+ "obstacle_proximity_upper_bound": 0.5
+ },
+
+ "optim": {
+ "no_inner_iterations": 5,
+ "no_outer_iterations": 4,
+ "optimization_activate": true,
+ "optimization_verbose": false,
+ "penalty_epsilon": 0.05,
+ "weight_max_vel_x": 2,
+ "weight_max_vel_y": 2,
+ "weight_max_vel_theta": 1,
+ "weight_acc_lim_x": 1,
+ "weight_acc_lim_y": 1,
+ "weight_acc_lim_theta": 1,
+ "weight_kinematics_nh": 1000,
+ "weight_kinematics_forward_drive": 1,
+ "weight_kinematics_turning_radius": 1,
+ "weight_optimaltime": 1,
+ "weight_shortest_path": 1,
+ "weight_obstacle": 50,
+ "weight_inflation": 0.1,
+ "weight_dynamic_obstacle": 50,
+ "weight_dynamic_obstacle_inflation": 0.1,
+ "weight_velocity_obstacle_ratio": 0,
+ "weight_viapoint": 1,
+ "weight_prefer_rotdir": 50,
+ "weight_adapt_factor": 2.0,
+ "obstacle_cost_exponent": 1.0
+ },
+
+ "hcp": {
+ "enable_multithreading": true,
+ "simple_exploration": false,
+ "max_number_classes": 5,
+ "max_number_plans_in_current_class": 0,
+ "selection_cost_hysteresis": 1.0,
+ "selection_obst_cost_scale": 100.0,
+ "selection_viapoint_cost_scale": 1.0,
+ "selection_alternative_time_cost": false,
+ "selection_dropping_probability": 0.0,
+ "switching_blocking_period": 0.0,
+
+ "obstacle_heading_threshold": 0.45,
+ "roadmap_graph_no_samples": 20,
+ "roadmap_graph_area_width": 10,
+ "roadmap_graph_area_length_scale": 1.0,
+ "h_signature_prescaler": 1,
+ "h_signature_threshold": 0.1,
+
+ "viapoints_all_candidates": true,
+
+ "delete_detours_backwards": true,
+ "detours_orientation_tolerance": 1.5707963267948966,
+ "length_start_orientation_vector": 0.4,
+ "max_ratio_detours_duration_best_duration": 3.0
+ }
+ }
+}
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index dfd3bf35aa932b14d0087041740aa2c1f2bff40b..ca819773b2c3e075d3303fc0bdc1cbc2ed0be9bf 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/source/armarx/navigation/CMakeLists.txt
@@ -15,6 +15,7 @@ add_subdirectory(location)
add_subdirectory(memory)
add_subdirectory(server)
add_subdirectory(platform_controller)
+add_subdirectory(human)
# Components.
add_subdirectory(components)
diff --git a/source/armarx/navigation/core/Trajectory.cpp b/source/armarx/navigation/core/Trajectory.cpp
index e1774afada179dc94358eacbb3d603fe2475cc1b..fc5c9b0ba2d7d2efc9516e1979ee04c62115517a 100644
--- a/source/armarx/navigation/core/Trajectory.cpp
+++ b/source/armarx/navigation/core/Trajectory.cpp
@@ -6,18 +6,6 @@
#include <iterator>
#include <limits>
-#include <range/v3/action/insert.hpp>
-#include <range/v3/numeric/accumulate.hpp>
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/all.hpp>
-#include <range/v3/view/concat.hpp>
-#include <range/v3/view/for_each.hpp>
-#include <range/v3/view/group_by.hpp>
-#include <range/v3/view/reverse.hpp>
-#include <range/v3/view/sliding.hpp>
-#include <range/v3/view/transform.hpp>
-#include <range/v3/view/zip.hpp>
-
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -34,6 +22,17 @@
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
+#include <range/v3/action/insert.hpp>
+#include <range/v3/numeric/accumulate.hpp>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/all.hpp>
+#include <range/v3/view/concat.hpp>
+#include <range/v3/view/for_each.hpp>
+#include <range/v3/view/group_by.hpp>
+#include <range/v3/view/reverse.hpp>
+#include <range/v3/view/sliding.hpp>
+#include <range/v3/view/transform.hpp>
+#include <range/v3/view/zip.hpp>
// FIXME move to simox
@@ -158,13 +157,14 @@ namespace armarx::navigation::core
} // namespace conv
- Projection
- GlobalTrajectory::getProjection(const Position& point,
- const VelocityInterpolation& velocityInterpolation) const
+
+ GlobalTrajectory::InternalProjection
+ GlobalTrajectory::projectInternal(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const
{
float distance = std::numeric_limits<float>::max();
- Projection bestProj;
+ InternalProjection bestProj;
for (size_t i = 0; i < pts.size() - 1; i++)
{
@@ -198,26 +198,9 @@ namespace armarx::navigation::core
math::LinearInterpolatedPose ip(
wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
- bestProj.wayPointBefore = wpBefore;
- bestProj.wayPointAfter = wpAfter;
+ bestProj.indexBefore = i;
bestProj.projection.waypoint.pose = ip.Get(t);
- bestProj.segment = [&]
- {
- if (i == 0)
- {
- return Projection::Segment::FIRST;
- }
-
- if (i == (pts.size() - 2))
- {
- return Projection::Segment::FINAL;
- }
-
- return Projection::Segment::INTERMEDIATE;
- }();
-
-
switch (velocityInterpolation)
{
case VelocityInterpolation::LinearInterpolation:
@@ -236,6 +219,82 @@ namespace armarx::navigation::core
return bestProj;
}
+ Projection
+ GlobalTrajectory::getProjection(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const
+ {
+ InternalProjection intProj = projectInternal(point, velocityInterpolation);
+
+ const auto& wpBefore = pts.at(intProj.indexBefore);
+ const auto& wpAfter = pts.at(intProj.indexBefore + 1);
+
+ Projection bestProj;
+
+ bestProj.wayPointBefore = wpBefore;
+ bestProj.wayPointAfter = wpAfter;
+ bestProj.projection = intProj.projection;
+
+ bestProj.segment = [&]
+ {
+ if (intProj.indexBefore == 0)
+ {
+ return Projection::Segment::FIRST;
+ }
+
+ if (intProj.indexBefore == (pts.size() - 2))
+ {
+ return Projection::Segment::FINAL;
+ }
+
+ return Projection::Segment::INTERMEDIATE;
+ }();
+
+ return bestProj;
+ }
+
+ std::pair<GlobalTrajectory, bool>
+ GlobalTrajectory::getSubTrajectory(const Position& point, float distance) const
+ {
+ const InternalProjection intProj =
+ projectInternal(point, VelocityInterpolation::LinearInterpolation);
+
+ GlobalTrajectory subTraj{};
+ subTraj.mutablePoints().push_back(intProj.projection);
+
+ float remainingDistance = distance;
+ GlobalTrajectoryPoint lastWp = intProj.projection;
+ for (size_t i = intProj.indexBefore + 1; i < pts.size(); i++)
+ {
+ const auto& nextWp = pts.at(i);
+
+ const float segmentDistance =
+ (nextWp.waypoint.pose.translation() - lastWp.waypoint.pose.translation()).norm();
+
+ if (segmentDistance < remainingDistance)
+ {
+ subTraj.mutablePoints().push_back(nextWp);
+ lastWp = nextWp;
+ remainingDistance -= segmentDistance;
+ }
+ else
+ {
+ // fraction of distance between segment end points
+ const float t = remainingDistance / segmentDistance;
+
+ math::LinearInterpolatedPose ip(
+ lastWp.waypoint.pose.matrix(), nextWp.waypoint.pose.matrix(), 0, 1, true);
+ const float velocity = lastWp.velocity + (nextWp.velocity - lastWp.velocity) * t;
+
+ subTraj.mutablePoints().push_back({{static_cast<Pose>(ip.Get(t))}, velocity});
+
+ return {subTraj, false};
+ }
+ }
+
+ // remaining trajectory is shorter than the specified distance
+ return {subTraj, true};
+ }
+
std::vector<Eigen::Vector3f>
GlobalTrajectory::positions() const noexcept
{
@@ -273,9 +332,9 @@ namespace armarx::navigation::core
GlobalTrajectory
GlobalTrajectory::FromPath(const Pose& start,
- const Positions& waypoints,
- const Pose& goal,
- const float velocity)
+ const Positions& waypoints,
+ const Pose& goal,
+ const float velocity)
{
// currently, only 2D version
@@ -331,7 +390,8 @@ namespace armarx::navigation::core
{
core::Positions resampledPath;
- const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose { return wp.waypoint.pose; };
+ const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose
+ { return wp.waypoint.pose; };
const core::Path originalPath = pts | ranges::views::transform(toPoint) | ranges::to_vector;
@@ -601,7 +661,7 @@ namespace armarx::navigation::core
constexpr int nSamples = 100;
- for(int j = 0; j < nSamples; j++)
+ for (int j = 0; j < nSamples; j++)
{
const float progress = static_cast<float>(j) / nSamples;
@@ -675,7 +735,6 @@ namespace armarx::navigation::core
return indices;
}
-
const std::vector<LocalTrajectoryPoint>&
LocalTrajectory::points() const
{
@@ -714,14 +773,16 @@ namespace armarx::navigation::core
// fraction of time between segment end points
const float t = d1 / dT;
- math::LinearInterpolatedPose ip(global_T_wp_before.matrix(), global_T_wp_after.matrix(), 0, 1, true);
+ math::LinearInterpolatedPose ip(
+ global_T_wp_before.matrix(), global_T_wp_after.matrix(), 0, 1, true);
const core::Pose wp_before_T_wp_after = global_T_wp_before.inverse() * global_T_wp_after;
const Eigen::Matrix3f& global_R_wp_before = global_T_wp_before.linear();
// the movement direction in the global frame
- const Eigen::Vector3f global_V_linear_movement_direction = global_R_wp_before * wp_before_T_wp_after.translation().normalized();
+ const Eigen::Vector3f global_V_linear_movement_direction =
+ global_R_wp_before * wp_before_T_wp_after.translation().normalized();
const Eigen::Vector3f distance = lower[1].pose.translation() - lower->pose.translation();
const float linearVelocity = static_cast<float>(distance.norm()) / dT.toSecondsDouble();
@@ -730,11 +791,9 @@ namespace armarx::navigation::core
Eigen::AngleAxisf angleAx(wp_before_T_wp_after.linear());
- const core::Twist feedforwardTwist
- {
+ const core::Twist feedforwardTwist{
.linear = linearVelocity * global_V_linear_movement_direction,
- .angular = angleAx.axis() * angleAx.angle() / dT.toSecondsDouble()
- };
+ .angular = angleAx.axis() * angleAx.angle() / dT.toSecondsDouble()};
return {static_cast<core::Pose>(ip.Get(t)), feedforwardTwist};
}
diff --git a/source/armarx/navigation/core/Trajectory.h b/source/armarx/navigation/core/Trajectory.h
index 5b2ba33ab9482879bbc4e713286074f4a0de32cb..20da0ba1f577865ab3e7938a1dddb4f3e565275a 100644
--- a/source/armarx/navigation/core/Trajectory.h
+++ b/source/armarx/navigation/core/Trajectory.h
@@ -24,6 +24,7 @@
#include <cstddef>
#include <memory>
+
#include "ArmarXCore/core/time/DateTime.h"
#include <armarx/navigation/core/basic_types.h>
@@ -76,6 +77,15 @@ namespace armarx::navigation::core
Projection getProjection(const Position& point,
const VelocityInterpolation& velocityInterpolation) const;
+ /**
+ * @brief Project point onto the trajectory and return a new trajectory along the old one
+ * from that point for the specified distance.
+ * @return The subtrajectory and whether the subtrajectory ends at the same point,
+ * as this trajectory
+ */
+ std::pair<GlobalTrajectory, bool> getSubTrajectory(const Position& point,
+ const float distance) const;
+
[[nodiscard]] std::vector<Position> positions() const noexcept;
[[nodiscard]] std::vector<Pose> poses() const noexcept;
@@ -117,6 +127,16 @@ namespace armarx::navigation::core
*/
Indices allConnectedPointsInRange(std::size_t idx, float radius) const;
+ private:
+ struct InternalProjection
+ {
+ GlobalTrajectoryPoint projection;
+ size_t indexBefore;
+ };
+ InternalProjection
+ projectInternal(const Position& point,
+ const VelocityInterpolation& velocityInterpolation) const;
+
private:
std::vector<GlobalTrajectoryPoint> pts;
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d8258b100e9a9e4c2aa745c160d7db8b344ca50d
--- /dev/null
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -0,0 +1,13 @@
+armarx_add_aron_library(human_aron
+ ARON_FILES
+)
+
+armarx_add_library(human
+ DEPENDENCIES_PUBLIC
+ armarx_navigation::core
+ DEPENDENCIES_PRIVATE
+ SOURCES
+ #types.cpp
+ HEADERS
+ types.h
+)
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
new file mode 100644
index 0000000000000000000000000000000000000000..8acd8c847be49512af82c7f3dfc0f5cc7b9e9c89
--- /dev/null
+++ b/source/armarx/navigation/human/types.h
@@ -0,0 +1,42 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/shapes.h>
+
+#include <armarx/navigation/core/basic_types.h>
+
+namespace armarx::navigation::human
+{
+ struct Human
+ {
+ core::Pose2D global_T_human;
+ Eigen::Vector2f linearVelocity;
+ };
+
+ struct HumanGroup
+ {
+ std::vector<Eigen::Vector2f> vertices;
+ std::vector<Human> humans;
+ };
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index f0495713816627b9a41ba4aa57cfd48c46cc791d..de0514f6e23f8aea568adb86499f07b59e41773d 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -1,11 +1,15 @@
#include "TimedElasticBands.h"
+#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/SceneObjectSet.h>
+#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h"
+
#include "armarx/navigation/conversions/eigen.h"
#include "armarx/navigation/core/Trajectory.h"
#include <armarx/navigation/local_planning/LocalPlanner.h>
@@ -30,23 +34,16 @@ namespace armarx::navigation::local_planning
aron::data::DictPtr
TimedElasticBandsParams::toAron() const
{
- arondto::TimedElasticBandsParams dto;
-
- TimedElasticBandsParams bo;
- armarx::navigation::local_planning::toAron(dto, bo);
-
- return dto.toAron();
+ return cfg.toAron();
}
TimedElasticBandsParams
TimedElasticBandsParams::FromAron(const aron::data::DictPtr& dict)
{
ARMARX_CHECK_NOT_NULL(dict);
- arondto::TimedElasticBandsParams dto;
- dto.fromAron(dict);
TimedElasticBandsParams bo;
- fromAron(dto, bo);
+ bo.cfg.fromAron(dict);
return bo;
}
@@ -54,12 +51,18 @@ namespace armarx::navigation::local_planning
// TimedElasticBands
- TimedElasticBands::TimedElasticBands(const Params& params, const core::Scene& ctx) :
- LocalPlanner(ctx), params(params), scene(ctx), obstManager(teb_obstacles)
+ TimedElasticBands::TimedElasticBands(const Params& i_params, const core::Scene& ctx) :
+ LocalPlanner(ctx), params(i_params), scene(ctx), obstManager(teb_obstacles)
{
- //TODO (SALt): init configuration, robot footprint
- initConfig();
- auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(0.2);
+ //TODO (SALt): find proper place to init with default config
+ if (true)
+ {
+ readDefaultConfig(params.cfg);
+ }
+ toTebCfg(params.cfg, cfg_);
+
+ auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(
+ params.cfg.robot_footprint_radius);
hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
hcp_->initialize(
@@ -69,47 +72,42 @@ namespace armarx::navigation::local_planning
}
void
- TimedElasticBands::initConfig()
+ TimedElasticBands::readDefaultConfig(arondto::TimedElasticBandsParams& target)
{
- cfg_.robot.max_vel_x = 1.0;
- cfg_.robot.max_vel_x_backwards = 1.0;
- cfg_.robot.max_vel_y = 1.0;
- cfg_.robot.max_vel_theta = 1.0;
- cfg_.robot.acc_lim_x = 1.0;
- cfg_.robot.acc_lim_y = 1.0;
- cfg_.robot.acc_lim_theta = 1.0;
-
- cfg_.optim.weight_max_vel_x = 1.0;
- cfg_.optim.weight_max_vel_y = 1.0;
- cfg_.optim.weight_max_vel_theta = 1.0;
- cfg_.optim.weight_acc_lim_x = 1.0;
- cfg_.optim.weight_acc_lim_y = 1.0;
- cfg_.optim.weight_acc_lim_theta = 1.0;
-
- cfg_.optim.weight_optimaltime = 0.6;
- cfg_.optim.weight_shortest_path = 0.6;
-
- cfg_.hcp.selection_cost_hysteresis = 1.0;
- cfg_.hcp.switching_blocking_period = 0;
-
- cfg_.pse.weight_global_path = 0.6;
-
- cfg_.trajectory.global_plan_overwrite_orientation = true;
+ const armarx::PackagePath configPath("armarx_navigation",
+ "local_planner_config/TimedElasticBands/default.json");
+ const std::filesystem::path file = configPath.toSystemPath();
+
+ ARMARX_CHECK(std::filesystem::is_regular_file(file)) << file;
+
+ ARMARX_INFO << "Loading config from file `" << file << "`.";
+ std::ifstream ifs{file};
+
+ nlohmann::json jsonConfig;
+ ifs >> jsonConfig;
+
+ ARMARX_INFO << "Reading config";
+
+ armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
+ target.read(reader, jsonConfig);
}
std::optional<LocalPlannerResult>
TimedElasticBands::plan(const core::GlobalTrajectory& goal)
{
- const teb_local_planner::TimedElasticBand globalPath = conv::toRos(goal);
+ const core::Pose currentPose{scene.robot->getGlobalPose()};
+
+ // prune global trajectory
+ const auto& [prunedGoal, planToDest] = goal.getSubTrajectory(
+ currentPose.translation(), params.cfg.planning_distance * 1000); // [m] to [mm]
+
+ const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal);
teb_globalPath = globalPath.poses();
hcp_->setGlobalPath(&teb_globalPath);
- const core::Pose currentPose{scene.robot->getGlobalPose()};
-
const teb_local_planner::PoseSE2 start = conv::toRos(currentPose);
-
- const FindTargetResult target = findTarget(currentPose, goal);
- const teb_local_planner::PoseSE2 end = conv::toRos(target.target);
+ const teb_local_planner::PoseSE2 end =
+ conv::toRos(prunedGoal.points().back().waypoint.pose);
geometry_msgs::Twist velocity_start = conv::toRos(scene.platformVelocity);
@@ -118,7 +116,7 @@ namespace armarx::navigation::local_planning
try
{
- hcp_->plan(start, end, &velocity_start, !target.planToDestination);
+ hcp_->plan(start, end, &velocity_start, !planToDest);
}
catch (std::exception& e)
{
@@ -134,20 +132,6 @@ namespace armarx::navigation::local_planning
}
- TimedElasticBands::FindTargetResult
- TimedElasticBands::findTarget(const core::Pose& currentPose,
- const core::GlobalTrajectory& globalPath)
- {
- // TODO (SALt): implement
-
- const core::Projection projection = globalPath.getProjection(
- currentPose.translation(), core::VelocityInterpolation::LinearInterpolation);
-
- return {conv::to2D(projection.projection.waypoint.pose),
- conv::to2D(globalPath.points().back().waypoint.pose),
- true};
- }
-
void
TimedElasticBands::fillObstacles()
{
@@ -155,7 +139,7 @@ namespace armarx::navigation::local_planning
if (scene.staticScene.has_value())
{
- for (auto& obst : scene.staticScene.value().objects->getCollisionModels())
+ for (const auto& obst : scene.staticScene.value().objects->getCollisionModels())
{
obstManager.addBoxObstacle(obst->getGlobalBoundingBox());
}
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.h b/source/armarx/navigation/local_planning/TimedElasticBands.h
index 26826650ec4ff1d7e667ba74dad48f3a0af39a04..9eba4d52739af83a79e0adcbc6cd8b0b19386b27 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.h
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.h
@@ -28,6 +28,7 @@
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/local_planning/TebObstacleManager.h>
+#include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
#include <armarx/navigation/local_planning/core.h>
#include <teb_local_planner/homotopy_class_planner.h>
#include <teb_local_planner/teb_config.h>
@@ -38,8 +39,7 @@ namespace armarx::navigation::local_planning
struct TimedElasticBandsParams : public LocalPlannerParams
{
- bool includeStartPose{false};
-
+ arondto::TimedElasticBandsParams cfg;
Algorithms algorithm() const override;
aron::data::DictPtr toAron() const override;
@@ -59,20 +59,11 @@ namespace armarx::navigation::local_planning
std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) override;
private:
- struct FindTargetResult
- {
- core::Pose2D projection;
- core::Pose2D target;
- bool planToDestination;
- };
-
- void initConfig();
- FindTargetResult findTarget(const core::Pose& currentPose,
- const core::GlobalTrajectory& globalPath);
+ void readDefaultConfig(arondto::TimedElasticBandsParams& target);
void fillObstacles();
protected:
- const Params params;
+ Params params;
private:
const core::Scene& scene;
diff --git a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
index 2c352aca2f9c548600b3093a847de474319b5972..4c3c924966bd6cdc02ec0b20eaf6f20d659993ba 100644
--- a/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
+++ b/source/armarx/navigation/local_planning/aron/TimedElasticBands.xml
@@ -5,10 +5,400 @@
</AronIncludes>
<GenerateTypes>
+ <Object name='armarx::navigation::local_planning::arondto::pse'>
+ <ObjectChild key='pse_costum_obstacle_penalties'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='pse_costum_obstacle_penalties_dynamic'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='weight_global_path'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='lrk_use_alternative_approach'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='lrk_enable_collision_check'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='hybrid_homotopy_calculation'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='robot_diff_circumscribed_inscribed_radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::trajectory'>
+ <ObjectChild key='teb_autosize'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='dt_ref'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='dt_hysteresis'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='min_samples'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='max_samples'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='global_plan_overwrite_orientation'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='allow_init_with_backwards_motion'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='exact_arc_length'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='force_reinit_new_goal_dist'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='force_reinit_new_goal_angular'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='feasibility_check_no_poses'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='feasibility_check_lookahead_distance'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='min_resolution_collision_check_angular'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::robot'>
+ <ObjectChild key='max_vel_x'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_vel_x_backwards'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_vel_y'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_vel_trans'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_vel_theta'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='acc_lim_x'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='acc_lim_y'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='acc_lim_theta'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='min_turning_radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::goal_tolerance'>
+ <ObjectChild key='xy_goal_tolerance'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::obstacles'>
+ <ObjectChild key='min_obstacle_dist'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='inflation_dist'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='dynamic_obstacle_inflation_dist'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='include_dynamic_obstacles'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_association_force_inclusion_factor'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_association_cutoff_factor'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_proximity_ratio_max_vel'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_proximity_lower_bound'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_proximity_upper_bound'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::optim'>
+ <ObjectChild key='no_inner_iterations'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='no_outer_iterations'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='optimization_activate'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='optimization_verbose'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='penalty_epsilon'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_max_vel_x'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_max_vel_y'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_max_vel_theta'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_acc_lim_x'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_acc_lim_y'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_acc_lim_theta'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_kinematics_nh'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_kinematics_forward_drive'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_kinematics_turning_radius'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_optimaltime'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_shortest_path'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_obstacle'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_inflation'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_dynamic_obstacle'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_dynamic_obstacle_inflation'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_velocity_obstacle_ratio'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_viapoint'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_prefer_rotdir'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='weight_adapt_factor'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='obstacle_cost_exponent'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::hcp'>
+ <ObjectChild key='enable_multithreading'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='simple_exploration'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='max_number_classes'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='max_number_plans_in_current_class'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='selection_cost_hysteresis'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='selection_obst_cost_scale'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='selection_viapoint_cost_scale'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='selection_alternative_time_cost'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='selection_dropping_probability'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='switching_blocking_period'>
+ <float />
+ </ObjectChild>
+
+
+ <ObjectChild key='obstacle_heading_threshold'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='roadmap_graph_no_samples'>
+ <int />
+ </ObjectChild>
+
+ <ObjectChild key='roadmap_graph_area_width'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='roadmap_graph_area_length_scale'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='h_signature_prescaler'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='h_signature_threshold'>
+ <float />
+ </ObjectChild>
+
+
+ <ObjectChild key='viapoints_all_candidates'>
+ <bool />
+ </ObjectChild>
+
+
+ <ObjectChild key='delete_detours_backwards'>
+ <bool />
+ </ObjectChild>
+
+ <ObjectChild key='detours_orientation_tolerance'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='length_start_orientation_vector'>
+ <float />
+ </ObjectChild>
+
+ <ObjectChild key='max_ratio_detours_duration_best_duration'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::local_planning::arondto::teb_config'>
+ <ObjectChild key='pse'>
+ <armarx::navigation::local_planning::arondto::pse />
+ </ObjectChild>
+ <ObjectChild key='trajectory'>
+ <armarx::navigation::local_planning::arondto::trajectory />
+ </ObjectChild>
+ <ObjectChild key='robot'>
+ <armarx::navigation::local_planning::arondto::robot />
+ </ObjectChild>
+ <ObjectChild key='goal_tolerance'>
+ <armarx::navigation::local_planning::arondto::goal_tolerance />
+ </ObjectChild>
+ <ObjectChild key='obstacles'>
+ <armarx::navigation::local_planning::arondto::obstacles />
+ </ObjectChild>
+ <ObjectChild key='optim'>
+ <armarx::navigation::local_planning::arondto::optim />
+ </ObjectChild>
+ <ObjectChild key='hcp'>
+ <armarx::navigation::local_planning::arondto::hcp />
+ </ObjectChild>
+ </Object>
+
<Object name='armarx::navigation::local_planning::arondto::TimedElasticBandsParams'>
- <ObjectChild key='foo'>
+ <ObjectChild key='robot_footprint_radius'>
+ <float />
+ </ObjectChild>
+ <ObjectChild key='planning_distance'>
<float />
</ObjectChild>
+ <ObjectChild key='teb_config'>
+ <armarx::navigation::local_planning::arondto::teb_config />
+ </ObjectChild>
</Object>
+
+
+
</GenerateTypes>
</AronTypeDefinition>
diff --git a/source/armarx/navigation/local_planning/aron_conversions.cpp b/source/armarx/navigation/local_planning/aron_conversions.cpp
index 44599c44b4d1953ba5d26cc557c07863895d1a4c..220a02b3bfb94f339db205a0e7167e7b78ca55b7 100644
--- a/source/armarx/navigation/local_planning/aron_conversions.cpp
+++ b/source/armarx/navigation/local_planning/aron_conversions.cpp
@@ -14,11 +14,141 @@ namespace armarx::navigation::local_planning
void
toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo)
{
+ dto = bo.cfg;
}
void
fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo)
{
+ bo.cfg = dto;
+ }
+
+ void
+ toTebCfg(const arondto::TimedElasticBandsParams& dto, teb_local_planner::TebConfig& bo)
+ {
+ bo.pse.pse_costum_obstacle_penalties = dto.teb_config.pse.pse_costum_obstacle_penalties;
+ bo.pse.pse_costum_obstacle_penalties_dynamic =
+ dto.teb_config.pse.pse_costum_obstacle_penalties_dynamic;
+ bo.pse.weight_global_path = dto.teb_config.pse.weight_global_path;
+ bo.pse.lrk_use_alternative_approach = dto.teb_config.pse.lrk_use_alternative_approach;
+ bo.pse.lrk_enable_collision_check = dto.teb_config.pse.lrk_enable_collision_check;
+ bo.pse.hybrid_homotopy_calculation = dto.teb_config.pse.hybrid_homotopy_calculation;
+ bo.pse.robot_diff_circumscribed_inscribed_radius =
+ dto.teb_config.pse.robot_diff_circumscribed_inscribed_radius;
+
+ // Trajectory
+ bo.trajectory.teb_autosize = bo.trajectory.teb_autosize;
+ bo.trajectory.dt_ref = dto.teb_config.trajectory.dt_ref;
+ bo.trajectory.dt_hysteresis = dto.teb_config.trajectory.dt_hysteresis;
+ bo.trajectory.min_samples = dto.teb_config.trajectory.min_samples;
+ bo.trajectory.max_samples = dto.teb_config.trajectory.max_samples;
+ bo.trajectory.global_plan_overwrite_orientation =
+ dto.teb_config.trajectory.global_plan_overwrite_orientation;
+ bo.trajectory.allow_init_with_backwards_motion =
+ dto.teb_config.trajectory.allow_init_with_backwards_motion;
+ bo.trajectory.exact_arc_length = dto.teb_config.trajectory.exact_arc_length;
+ bo.trajectory.force_reinit_new_goal_dist =
+ dto.teb_config.trajectory.force_reinit_new_goal_dist;
+ bo.trajectory.force_reinit_new_goal_angular =
+ dto.teb_config.trajectory.force_reinit_new_goal_angular;
+ bo.trajectory.feasibility_check_no_poses =
+ dto.teb_config.trajectory.feasibility_check_no_poses;
+ bo.trajectory.feasibility_check_lookahead_distance =
+ dto.teb_config.trajectory.feasibility_check_lookahead_distance;
+ bo.trajectory.min_resolution_collision_check_angular =
+ dto.teb_config.trajectory.min_resolution_collision_check_angular;
+
+ // Robot
+ bo.robot.max_vel_x = dto.teb_config.robot.max_vel_x;
+ bo.robot.max_vel_x_backwards = dto.teb_config.robot.max_vel_x_backwards;
+ bo.robot.max_vel_y = dto.teb_config.robot.max_vel_y;
+ bo.robot.max_vel_trans = dto.teb_config.robot.max_vel_trans;
+ bo.robot.max_vel_theta = dto.teb_config.robot.max_vel_theta;
+ bo.robot.acc_lim_x = dto.teb_config.robot.acc_lim_x;
+ bo.robot.acc_lim_y = dto.teb_config.robot.acc_lim_y;
+ bo.robot.acc_lim_theta = dto.teb_config.robot.acc_lim_theta;
+ bo.robot.min_turning_radius = dto.teb_config.robot.min_turning_radius;
+
+ // GoalTolerance
+ bo.goal_tolerance.xy_goal_tolerance = dto.teb_config.goal_tolerance.xy_goal_tolerance;
+
+ // Obstacles
+ bo.obstacles.min_obstacle_dist = dto.teb_config.obstacles.min_obstacle_dist;
+ bo.obstacles.inflation_dist = dto.teb_config.obstacles.inflation_dist;
+ bo.obstacles.dynamic_obstacle_inflation_dist =
+ dto.teb_config.obstacles.dynamic_obstacle_inflation_dist;
+ bo.obstacles.include_dynamic_obstacles = dto.teb_config.obstacles.include_dynamic_obstacles;
+ bo.obstacles.obstacle_association_force_inclusion_factor =
+ dto.teb_config.obstacles.obstacle_association_force_inclusion_factor;
+ bo.obstacles.obstacle_association_cutoff_factor =
+ dto.teb_config.obstacles.obstacle_association_cutoff_factor;
+ bo.obstacles.obstacle_proximity_ratio_max_vel =
+ dto.teb_config.obstacles.obstacle_proximity_ratio_max_vel;
+ bo.obstacles.obstacle_proximity_lower_bound =
+ dto.teb_config.obstacles.obstacle_proximity_lower_bound;
+ bo.obstacles.obstacle_proximity_upper_bound =
+ dto.teb_config.obstacles.obstacle_proximity_upper_bound;
+
+ // Optimization
+ bo.optim.no_inner_iterations = dto.teb_config.optim.no_inner_iterations;
+ bo.optim.no_outer_iterations = dto.teb_config.optim.no_outer_iterations;
+ bo.optim.optimization_activate = dto.teb_config.optim.optimization_activate;
+ bo.optim.optimization_verbose = dto.teb_config.optim.optimization_verbose;
+ bo.optim.penalty_epsilon = dto.teb_config.optim.penalty_epsilon;
+ bo.optim.weight_max_vel_x = dto.teb_config.optim.weight_max_vel_x;
+ bo.optim.weight_max_vel_y = dto.teb_config.optim.weight_max_vel_y;
+ bo.optim.weight_max_vel_theta = dto.teb_config.optim.weight_max_vel_theta;
+ bo.optim.weight_acc_lim_x = dto.teb_config.optim.weight_acc_lim_x;
+ bo.optim.weight_acc_lim_y = dto.teb_config.optim.weight_acc_lim_y;
+ bo.optim.weight_acc_lim_theta = dto.teb_config.optim.weight_acc_lim_theta;
+ bo.optim.weight_kinematics_nh = dto.teb_config.optim.weight_kinematics_nh;
+ bo.optim.weight_kinematics_forward_drive =
+ dto.teb_config.optim.weight_kinematics_forward_drive;
+ bo.optim.weight_kinematics_turning_radius =
+ dto.teb_config.optim.weight_kinematics_turning_radius;
+ bo.optim.weight_optimaltime = dto.teb_config.optim.weight_optimaltime;
+ bo.optim.weight_shortest_path = dto.teb_config.optim.weight_shortest_path;
+ bo.optim.weight_obstacle = dto.teb_config.optim.weight_obstacle;
+ bo.optim.weight_inflation = dto.teb_config.optim.weight_inflation;
+ bo.optim.weight_dynamic_obstacle = dto.teb_config.optim.weight_dynamic_obstacle;
+ bo.optim.weight_dynamic_obstacle_inflation =
+ dto.teb_config.optim.weight_dynamic_obstacle_inflation;
+ bo.optim.weight_velocity_obstacle_ratio =
+ dto.teb_config.optim.weight_velocity_obstacle_ratio;
+ bo.optim.weight_viapoint = dto.teb_config.optim.weight_viapoint;
+ bo.optim.weight_prefer_rotdir = dto.teb_config.optim.weight_prefer_rotdir;
+ bo.optim.weight_adapt_factor = dto.teb_config.optim.weight_adapt_factor;
+ bo.optim.obstacle_cost_exponent = dto.teb_config.optim.obstacle_cost_exponent;
+
+ // Homotopy Class Planner
+ bo.hcp.enable_multithreading = dto.teb_config.hcp.enable_multithreading;
+ bo.hcp.simple_exploration = dto.teb_config.hcp.simple_exploration;
+ bo.hcp.max_number_classes = dto.teb_config.hcp.max_number_classes;
+ bo.hcp.max_number_plans_in_current_class =
+ dto.teb_config.hcp.max_number_plans_in_current_class;
+ bo.hcp.selection_cost_hysteresis = dto.teb_config.hcp.selection_cost_hysteresis;
+ bo.hcp.selection_obst_cost_scale = dto.teb_config.hcp.selection_obst_cost_scale;
+ bo.hcp.selection_viapoint_cost_scale = dto.teb_config.hcp.selection_viapoint_cost_scale;
+ bo.hcp.selection_alternative_time_cost = dto.teb_config.hcp.selection_alternative_time_cost;
+ bo.hcp.selection_dropping_probability = dto.teb_config.hcp.selection_dropping_probability;
+ bo.hcp.switching_blocking_period = dto.teb_config.hcp.switching_blocking_period;
+
+ bo.hcp.obstacle_heading_threshold = dto.teb_config.hcp.obstacle_heading_threshold;
+ bo.hcp.roadmap_graph_no_samples = dto.teb_config.hcp.roadmap_graph_no_samples;
+ bo.hcp.roadmap_graph_area_width = dto.teb_config.hcp.roadmap_graph_area_width;
+ bo.hcp.roadmap_graph_area_length_scale = dto.teb_config.hcp.roadmap_graph_area_length_scale;
+ bo.hcp.h_signature_prescaler = dto.teb_config.hcp.h_signature_prescaler;
+ bo.hcp.h_signature_threshold = dto.teb_config.hcp.h_signature_threshold;
+
+ bo.hcp.viapoints_all_candidates = dto.teb_config.hcp.viapoints_all_candidates;
+
+ bo.hcp.delete_detours_backwards = dto.teb_config.hcp.delete_detours_backwards;
+ bo.hcp.detours_orientation_tolerance = dto.teb_config.hcp.detours_orientation_tolerance;
+ bo.hcp.length_start_orientation_vector = dto.teb_config.hcp.length_start_orientation_vector;
+ bo.hcp.max_ratio_detours_duration_best_duration =
+ dto.teb_config.hcp.max_ratio_detours_duration_best_duration;
+
+ bo.checkParameters();
}
diff --git a/source/armarx/navigation/local_planning/aron_conversions.h b/source/armarx/navigation/local_planning/aron_conversions.h
index 1b7d2382255a542a15fa9066a0a83348ec1e2fb2..22eb26d1a468e9ce7f56027e2bb00b28767b3d27 100644
--- a/source/armarx/navigation/local_planning/aron_conversions.h
+++ b/source/armarx/navigation/local_planning/aron_conversions.h
@@ -1,5 +1,7 @@
#pragma once
+#include <teb_local_planner/teb_config.h>
+
namespace armarx::navigation::local_planning
{
// struct LocalPlannerParams;
@@ -19,4 +21,6 @@ namespace armarx::navigation::local_planning
void toAron(arondto::TimedElasticBandsParams& dto, const TimedElasticBandsParams& bo);
void fromAron(const arondto::TimedElasticBandsParams& dto, TimedElasticBandsParams& bo);
+ void toTebCfg(const arondto::TimedElasticBandsParams& dto, teb_local_planner::TebConfig& bo);
+
} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 3ad81d9c24bf20736e15b5473689a1f585b421ac..c23adf1af93c6eaa9196f8398e6c5f5f8e764ba3 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -18,7 +18,7 @@ namespace armarx::navigation::conv
Eigen::Vector2d
toRos(const Eigen::Vector3f& vec)
{
- return conv::to2D(vec).cast<double>() / 1000;
+ return conv::to2D(vec).cast<double>() / 1000; // [mm] to [m]
}
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 694ff174eb4569e437b3d6c997f4ed69d0ac354a..cb1f306e636a84d57a244620bbcbde7380ada2ae 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -172,7 +172,7 @@ namespace armarx::navigation::server
simox::ColorMap cm = simox::color::cmaps::inferno();
cm.set_vmin(0);
- cm.set_vmax(1);
+ cm.set_vmax(0.6);
for (size_t i = 0; i < trajectory.points().size() - 1; i++)
{