rayvburn / humap_local_planner

Human-aware robot trajectory planner using a hybrid trajectory candidates generation and spatiotemporal cost functions

humap_local_planner

A local planning module for A Framework to Simulate Human Behaviour in Robot Research

Planner behaviour

Finite state machine of the planner is shown in the following diagram:

@startuml
state "Initiate Movement" as init
state "Move" as move
state "Adjust Orientation" as adjust
state "Stop" as stop

[*] --> init
init --> move : reached orientation \n of the first pose in a global plan \n (30 degrees tolerance)
move --> adjust : \treached goal position
adjust --> stop : \treached goal orientation
stop --> init : received new \n goal pose
@enduml

Prerequisites

apt packages

sudo apt install ros-$ROS_DISTRO-costmap-converter

teb_local_planner

Slightly modified teb_local_planner (its 'utils' in particular) is currently required for operation:

ROS Kinetic

git clone -b robot-footprint-model-access-fix [email protected]:rayvburn/teb_local_planner.git

ROS Melodic:

git clone -b melodic-devel-robot-footprint-model-access-fix [email protected]:rayvburn/teb_local_planner.git

fuzzylite

Additionally, fuzzylite library built from source is required to run humap_local_planner. Follow these steps:

cd ~/
mkdir libraries
cd libraries
git clone https://github.com/fuzzylite/fuzzylite.git

The run fuzzylite/build.sh script as pointed in library build instructions.

Disclaimer

This research code is provided with the intention of being helpful, but it comes with no guarantees; neither the implied warranty of merchantability nor fitness for any specific purpose is provided.

humap_local_planner's Issues

port to ROS Melodic on Ubuntu Bionic

• ROS Melodic Morenia
• Ubuntu 18.04.6 LTS (Bionic)

since real TIAGo runs this version

publish force vectors lengths as point cloud

smoothen behaviour forces

consider closest distance to dynamic object and sharpened fuzzy value

add `VelocitySmoothnessCostFunction`

try to keep linear velocity constant if possible

investigate why `sim_period` change affects force vectors lengths

robot treats global path's end point as local goal

verify if corrent

link `fuzzylite` library in a nice way

# fuzzylite library - external dependency
# path to the .so file
find_library(fuzzylite_LIBRARY fuzzylite $ENV{HOME}/libraries/fuzzylite/fuzzylite/release/bin)
# include fuzzylite's headers folder
include_directories($ENV{HOME}/libraries/fuzzylite/fuzzylite)

This is ugly and requires building from source. Use version installed via apt.

massive freezes during execution (simulation with `Gazebo`)

I've encountered massive freezes with Gazebo simulation in a pretty simple world.

EDIT: I've tested other planners (DWA, TEB) and situation is similar. It may be some issue with my PC? Nonetheless, TEB seems to execute approx 10 x faster than HuBeRo scoring ~100 trajectories in each step. It may be also caused by extensive logging of HuBeRo (and no logs passed to the console out from TEB).

Checked at the move_base level:

      ros::WallDuration t_diff = ros::WallTime::now() - start;
      ros::Duration t_diff_sim = ros::Time::now() - start_sim;
      ROS_INFO_NAMED("move_base","Full control cycle time: real {%.9f}, sim {%.9f}\n", t_diff.toSec(), t_diff_sim.toSec());

update fuzzy inference system computations

representation of the output value has changed

robot footprint class extension adjustments

calculateShortestVector and calculateClosestPoints methods in LineRobotFootprint are not implemented great (they need testing, also)

Additionally, testing is needed in:

• TwoCirclesRobotFootprint
• PolygonRobotFootprint

create initial version of better positioning for goal orientation

Currently robot suffers from excessive rotation after goal position is reached. Use, e.g., base_local_planner::LatchedStopRotateController

velocity saturation

• saturate velocity to fit kinematic constraints
• create a function that smoothly saturates velocity based on presence of dynamic objects nearby

Fix `hubero_common`'s `converter` moved to `hubero_local_planner` workaround

• https://answers.ros.org/question/237183/the-library-is-neither-a-target-nor-builtinstalled-properly/
• https://answers.ros.org/question/97606/export-a-prebuilt-library-in-catkin/
• https://answers.ros.org/question/130301/catkin-how-to-set-up-build-dependencies-of-two-packages-correctly/

  add_dependencies(myexecutable ${catkin_EXPORTED_TARGETS})

comparison with DWA and TEB

• prepare comparison of planner performance compared with DWA and TEB

update force parameters

based on tuning in simulations and results from article

consider people personal space

• subscribe a topic feeding people data
• model personal space
  • refer to Proxemics models for human-aware navigation in robotics: Grounding interaction and personal space models in experimental data from psychology, Barnaud et. al, 2014
• visualize people (use iri_atr_akp_local_planner_companion approach?)
• consider personal space in planning

error in distant pose cost calculation `HuberoPlannerROS::createCostGridPcl`

Added this section to HuberoPlannerROS::createCostGridPcl:

	geometry_msgs::PoseStamped pose_goal;
	planner_util_->getGoal(pose_goal);
	planner_util_->getCostmap()->worldToMapNoBounds(
		pose_goal.pose.position.x,
		pose_goal.pose.position.y,
		mx,
		my
	);
	planner_->computeCellCost(mx, my, path_cost, goal_cost, occ_cost, total_cost); 

Then, sometimes when mx or my is negative, HuberoPlanner::computeCellCost fails at:

	path_costs_.getCellCosts(cx, cy);

and move_base process dies. Interestingly, program does not even enter MapGridCostFunction::getCellCosts (added logs are not printed).

This happens for goals placed in a long distance from robot base. Behaviour observed only when move_base is restarted while mobile base controllers are still running. Therefore it's not investigated further at that moment.

article coverage

SMooTh: smooth social force model approach for lightweight reactive navigation for differential-drive mobile bases

extend `base_local_planner::Trajectory` with force components and amplifiers (for visualization)

remove teb_local_planner dependency

• copy obstacles.h as external header
• add eigen library as dependency

differentiate interaction factor between static and dynamic objects

Related to, depends on #18

inspired by Fig. 1 @ Socially-Aware Navigation: A Non-linear Multi-Objective Optimization Approach

create a sequence diagram for a single sample in the planner

ref: sequence diagram

mark dynamic obstacles in visualization

consider acceleration limits in trajectory generation

define `computeTwist` that effectively drives robot straight to the goal when no obstacles are present

Previous title: apply angular velocity correction when defining ideal velocity for internal force

"Ideal velocity", i.e., the one that drives robot straight to the goal pose.
The differential drive robot typically has non-zero angular velocity. That is not accounted for in force calculations therefore some kind of correction, when computing so called ideal velocity vector, is necessary.

---

investigate causes of massive decelerations in cluttered areas of the world

First, check if increasing internal force factor will do any good.

avoiding local minima

• in exemplary world it's easy to achieve by requesting the robot to go towards the wall upfront

consider robot footprint in environment

Trapezoids calculation remake - eliminate specific cases, design a generic method

Currently there are specific cases, especially when intersection values are very big, when Trapezoid vertices are not calculated properly.

A good starting point:

bool TrapezoidParted::update(const double &start, const double &end) {
	// Creates parameter configuration equal to the uninitialized fl::Trapezoid instance
	std::string params("nan nan nan nan");
	std::string params_wrap("nan nan nan nan");

	Angle an(start - intersection_);
	Angle bn(an.getRadian() + intersection_, false);

	double top_length = 0.0;
	if (start >= end) {
		top_length = (IGN_PI - start) + (end - (-IGN_PI));
	} else {
		top_length = end - start;
	}
	Angle cn(bn.getRadian() + top_length, false);
	Angle dn(cn.getRadian() + intersection_, false);

	Angle shift(-IGN_PI);
	if (bn.getRadian() > IGN_PI) {
		shift = Angle(+IGN_PI);
	}
	Angle aw(an.getRadian() + shift.getRadian());
	Angle bw(bn.getRadian() + shift.getRadian());
	Angle cw(cn.getRadian() + shift.getRadian());
	Angle dw(dn.getRadian() + shift.getRadian());

	params = generateParams(an.getRadian(), bn.getRadian(), cn.getRadian(), dn.getRadian(), 1.0);
	params_wrap = generateParams(aw.getRadian(), bw.getRadian(), cw.getRadian(), dw.getRadian(), 1.0);
	trapezoid_ptrs_.at(0)->configure(params);
	trapezoid_ptrs_.at(1)->configure(params_wrap);

	return true;

Follow notes from 2021.11.12.
This change will most likely affect unit tests.

convert local velocity vector into global one

• base x, y, theta velocities into x, y ones in planner's frame

references:

• commented out TransformListener::transformTwist + failed review

Fix segmentation fault in `FuzzyInferenceSystem` unit tests

Related to fuzzylite/fuzzylite#93

goal reachment definition

plan in local coordinate frame

get rid of `teb_local_planner` dependency

our PR teb_local_planner#295 will not be merged so copy necessary utils from their code and extend accordingly

adjust default min/max values of `TrajectorySamplingParams` to match std deviation from `Seer` experiments

trajectory planning procedure

• initial version can have a sampling-based approach implemented (like DWA)

• additional cost to follow the global plan

• human trajectory prediction with a use of SFM & fuzzy logic but beware of oscillations (it's still a potential field), so constant velocity model may also be considered (superposition?)

references:

• critics discussion on ROS2 implementation: URL
• base_local_planner tools

comparison with other SFM-based planners

• formal analysis of possible enhancements that were applied in other approaches based on SFM, ref: local planners list

`HuberoConfig` cleanup - move structs with parameter definitions to relevant classes

pass footprint specification from `HuberoPlannerROS` to `HuberoPlanner`

create abstraction layer that contains different types used in package

Aim is to not rely strictly on ignition library or ROS geometry_msgs. Create ctors to convert between types like in converter

a.k.a. GEOMETRY REFACTOR

costmap marks the cell currently occupied by a robot as an obstacle

It happens sometimes, cannot really reproduce it off the shelf. Although it may be related to computation speed issues (vs costmap obstacle timings)? Happens when PC is under heavy load.

Effect is that mobile base receives zero velocity command.

Example:

add recovery behaviours

stick to teb_local_planner dependency

using namespace teb_local_planner;

instead of copying files

port algorithm

from actor_plugin_social

prepare environment representation used by sfm

SFM ontology

• create a struct that stores a vector of obstacles and robot footprint (+ necessary data) and relations between each pair
• unify types
• avoid unnecessary conversions

use costmap_converter

as teb_local_planner

use global plan for local navigation

• global plan usage

• place a local goal that stands for a goal for social force model calculations

• visualize global and local goals

• equip ontology with both global and local goals

• modify grid visualization so it does not require a new plan for every new marker (?)

convert force vectors into velocity commands (linear and angular)

force <-> velocity commands

better costmap_converter segmentation plugin

• modify costmap_converter::CostmapToLinesDBSRANSAC

unit tests for dir cross regions