nobleo/path_tracking_pid

Path Tracking PID offers a tuneable PID control loop, decouling steering and forward velocity

path_tracking_pid's Issues

Build fails when CATKIN_ENABLE_TESTING=OFF

Steps to reproduce:

• set -DCATKIN_ENABLE_TESTING=OFF in catkin config-command
• clean build folders of ROS
• run catkin build
• the following error-output is provided
  
  and seems to be related to https://github.com/nobleo/path_tracking_pid/blob/main/CMakeLists.txt#L81

Controller's end phase distance is calculated wrong for backwards motion

d_end_phase (here) can result in negative values if target_x_vel < 0

Don't append the global plan

We append the global plan with the carrot distance here:

path_tracking_pid/src/controller.cpp

Lines 190 to 196 in 61c6cc8

	if (!config_.track_base_link) {
	// Add carrot length to plan using goal pose (we assume the last pose contains correct angle)
	tf2::Transform carrotTF(
	tf2::Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0),
	tf2::Vector3(config_.l, 0.0, 0.0));
	global_plan_tf_.push_back(global_plan_tf_.back() * carrotTF);
	}

But there are a few problems with this:

• this distance may change at runtime (dynamic reconfigure), and this is not reflected in the global plan,
• the last pose may not be in the direction of the end of the plan, making the direction in which we append the plan rather arbitrary,

Global plan is changed to the controller's liking

IMO, if the controller is incapable of handling the global plan, it should reject the global plan, not alter it to its liking. This filter_plan call does exactly that. I think we should just check it, and return REJECTED if it does not satisfy the controller's constraints to the global plan.

path_tracking_pid/src/controller.cpp

Lines 183 to 188 in 61c6cc8

	global_plan_tf_ = filter_plan(global_plan);
	if (global_plan_tf_.size() != global_plan.size()) {
	ROS_WARN(
	"Not all poses of path are used since not all poses were in the expected direction of the "
	"path!");
	}

Controller rejects goals when its last setpoint does not agree with odom velocity (on `init_vel_method` `pid_odom`)

Talking about this piece of code in setPlan(), intended to avoid putting steps on the velocity setpoint:

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 147 to 158 in 61c6cc8

	// Feasability check, but only when not resuming with odom-vel
	if (
	pid_controller_.getConfig().init_vel_method != Pid_Odom &&
	pid_controller_.getConfig().init_vel_max_diff >= 0.0 &&
	std::abs(
	latest_odom_.twist.twist.linear.x - pid_controller_.getControllerState().current_x_vel) >
	pid_controller_.getConfig().init_vel_max_diff) {
	ROS_ERROR(
	"Significant diff between odom (%f) and controller_state (%f) detected. Aborting!",
	latest_odom_.twist.twist.linear.x, pid_controller_.getControllerState().current_x_vel);
	return false;
	}

Currently, the controller rejects any new goal when the error between actual and setpoint velocity is too large. It is unknown what happens then, because the controller will not send any velocities anymore. So this depends on what third party control action or disturbance made this happen.

IMO, we should accept this goal (as there is nothing wrong with the goal itself. The goal doesn't say anything about velocities). In computeVelocityCommands, we should check that this error does not become too large. If that does happen, we should abort the goal, set the setpoint to the current odom and decrease that to zero, respecting our acceleration limits.

The behavior of the robot stays the same in the special case this was designed for: a manual override while controlling. Because if the actual velocity is zero, the error is too large, the goal is accepted, but it is aborted right away, controlling the velocity to zero (which it already is) and resetting the controller state.

But it should now also produce the desired behavior in all other cases: where the actual velocity is nonzero, but not similar to the controller state. The goal is accepted, but aborted due to a controller error. The velocity is smoothly decreased to zero, and the controller state is again in sync with the robot state.

Refactor geometry_msgs::* to tf2:: datatypes where possible in controller.hpp

• convert member variable geometry_msgs::Transform tf_base_to_steered_wheel_
• convert all geometry_msgs::Transform that are used as arguments
  • maybe start with setPlan, move the conversion code to the ROS wrapper
  • global_plan_ doesn't need to be a member

Controller state doesn't need to be reset on reconfigure

The controller can be reconfigured while driving. If the controller is also reset, then the D and I action are reset to 0, and the lowpass starts from scratch. This is unwanted behavior.

path_tracking_pid/src/controller.cpp

Line 978 in ff653a6

controller_state_.error_lat.reset();

Code review

1

Move base flex passes you the odometry at every call of computeVelocityCommands, so there is no need for this subscriber:

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Line 70 in 00202b4

sub_odom_ = gn.subscribe("odom", 1, &TrackingPidLocalPlanner::curOdomCallback, this);

2

The next piece of code checks for compatibility of the plan's frame and the frame the controller is running in. This makes assumptions about the application. Does the user want you to control in the plan's frame, or do they want the controller to convert the plan to the controller's frame? There's no way of knowing.

Keep the the controller (and the entire ROS system) clean by assuming you're running in the standardized frames (map, odom, or maybe utm or something). In this case you're using the frame specified for move_base_flex. If the user wants to be flexible with respect to the reference frames of navigation plans, they should transform any incoming plans to the move_base_flex global frame in a separate rosnode with the isolated functionality of providing this flexible interface.

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 96 to 117 in 00202b4

	if (map_frame_.compare(path_frame))
	{
	ROS_DEBUG("Transforming plan since my global_frame = '%s' and my plan is in frame: '%s'", map_frame_.c_str(),
	path_frame.c_str());
	geometry_msgs::TransformStamped tf_transform;
	tf_transform = tf_->lookupTransform(map_frame_, path_frame, ros::Time(0));
	// Check alignment, when path-frame is severly mis-aligned show error
	double yaw, pitch, roll;
	tf2::getEulerYPR(tf_transform.transform.rotation, yaw, pitch, roll);
	if (std::fabs(pitch) > MAP_PARALLEL_THRESH || std::fabs(roll) > MAP_PARALLEL_THRESH)
	{
	ROS_ERROR("Path is given in %s frame which is severly mis-aligned with our map-frame: %s", path_frame.c_str(),
	map_frame_.c_str());
	}
	for (auto& pose_stamped : global_plan_)
	{
	tf2::doTransform(pose_stamped.pose, pose_stamped.pose, tf_transform);
	pose_stamped.header.frame_id = map_frame_;
	// 'Project' plan by removing z-component
	pose_stamped.pose.position.z = 0.0;
	}
	}

3

In the next piece of code, the cmd_vel is set to the controller state:

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 193 to 194 in 00202b4

	cmd_vel.linear.x = pid_controller_.getControllerState().current_x_vel;
	cmd_vel.angular.z = pid_controller_.getControllerState().current_yaw_vel;

Why not just use the current odom twist, provided as function argument to the (calling) function?

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 252 to 265 in 00202b4

	if (cancel_requested_)
	{
	path_tracking_pid::PidConfig config = pid_controller_.getConfig();
	// Copysign here, such that when cancelling while driving backwards, we decelerate to -0.0 and hence
	// the sign propagates correctly
	config.target_x_vel = std::copysign(0.0, config.target_x_vel);
	config.target_end_x_vel = std::copysign(0.0, config.target_x_vel);
	boost::recursive_mutex::scoped_lock lock(config_mutex_);
	// When updating from own server no callback is called. Thus controller is updated first and then server is notified
	pid_controller_.configure(config);
	pid_server_->updateConfig(config);
	lock.unlock();
	cancel_requested_ = false;
	}

4

When a cancel is requested, the controller changes its own parameters

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 214 to 239 in 00202b4

	// Handle obstacles
	if (pid_controller_.getConfig().anti_collision)
	{
	auto cost = projectedCollisionCost();
	if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
	{
	pid_controller_.setVelMaxObstacle(0.0);
	}
	else if (pid_controller_.getConfig().obstacle_speed_reduction)
	{
	double max_vel = pid_controller_.getConfig().max_x_vel;
	double reduction_factor = static_cast<double>(cost) / costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
	double limit = max_vel * (1 - reduction_factor);
	ROS_DEBUG("Cost: %d, factor: %f, limit: %f", cost, reduction_factor, limit);
	pid_controller_.setVelMaxObstacle(limit);
	}
	else
	{
	pid_controller_.setVelMaxObstacle(INFINITY); // set back to inf
	}
	}
	else
	{
	pid_controller_.setVelMaxObstacle(INFINITY); // Can be disabled live, so set back to inf
	}

Why not just set another limit just like in the obstacle handling (just above these lines).

5

This next bit only constructs some visualization markers to publish for debugging purposes, but it takes up a lot of space in the file:

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 275 to 339 in 00202b4

	mkCurPose.header.frame_id = mkControlPose.header.frame_id = map_frame_;
	mkGoalPose.header.frame_id = mkPosOnPlan.header.frame_id = map_frame_;
	mkCurPose.header.stamp = mkControlPose.header.stamp = ros::Time::now();
	mkGoalPose.header.stamp = mkPosOnPlan.header.stamp = ros::Time::now();
	mkCurPose.ns = "axle point";
	mkControlPose.ns = "control point";
	mkGoalPose.ns = "goal point";
	mkPosOnPlan.ns = "plan point";
	mkCurPose.action = mkControlPose.action = visualization_msgs::Marker::ADD;
	mkGoalPose.action = mkPosOnPlan.action = visualization_msgs::Marker::ADD;
	mkCurPose.pose.orientation.w = mkControlPose.pose.orientation.w = 1.0;
	mkGoalPose.pose.orientation.w = mkPosOnPlan.pose.orientation.w = 1.0;
	mkCurPose.id = __COUNTER__; // id has to be unique, so using a compile-time counter :)
	mkControlPose.id = __COUNTER__;
	mkGoalPose.id = __COUNTER__;
	mkPosOnPlan.id = __COUNTER__;
	mkCurPose.type = mkControlPose.type = visualization_msgs::Marker::POINTS;
	mkGoalPose.type = mkPosOnPlan.type = visualization_msgs::Marker::POINTS;
	mkCurPose.scale.x = 0.5;
	mkCurPose.scale.y = 0.5;
	mkControlPose.scale.x = 0.5;
	mkControlPose.scale.y = 0.5;
	mkGoalPose.scale.x = 0.5;
	mkGoalPose.scale.y = 0.5;
	mkCurPose.color.b = 1.0;
	mkCurPose.color.a = 1.0;
	mkControlPose.color.g = 1.0f;
	mkControlPose.color.a = 1.0;
	mkGoalPose.color.r = 1.0;
	mkGoalPose.color.a = 1.0;
	mkPosOnPlan.scale.x = 0.5;
	mkPosOnPlan.scale.y = 0.5;
	mkPosOnPlan.color.a = 1.0;
	mkPosOnPlan.color.r = 1.0f;
	mkPosOnPlan.color.g = 0.5f;
	geometry_msgs::Point p;
	std_msgs::ColorRGBA color;
	p.x = tfCurPoseStamped_.transform.translation.x;
	p.y = tfCurPoseStamped_.transform.translation.y;
	p.z = tfCurPoseStamped_.transform.translation.z;
	mkCurPose.points.push_back(p);
	tf2::Transform tfControlPose = pid_controller_.getCurrentWithCarrot();
	p.x = tfControlPose.getOrigin().x();
	p.y = tfControlPose.getOrigin().y();
	p.z = tfControlPose.getOrigin().z();
	mkControlPose.points.push_back(p);
	tf2::Transform tfGoalPose = pid_controller_.getCurrentGoal();
	p.x = tfGoalPose.getOrigin().x();
	p.y = tfGoalPose.getOrigin().y();
	p.z = tfGoalPose.getOrigin().z();
	mkGoalPose.points.push_back(p);
	tf2::Transform tfCurPose = pid_controller_.getCurrentPosOnPlan();
	p.x = tfCurPose.getOrigin().x();
	p.y = tfCurPose.getOrigin().y();
	p.z = tfCurPose.getOrigin().z();
	mkPosOnPlan.points.push_back(p);
	marker_pub_.publish(mkCurPose);
	marker_pub_.publish(mkControlPose);
	marker_pub_.publish(mkGoalPose);
	marker_pub_.publish(mkPosOnPlan);

This clutters the implementation, and makes it harder to understand. I would suggest putting blocks of code like this (debug/visualization) in a separate file, so you can keep the code for actual functionality clean and concise.

6

A cancel request now blocks until the goal is no longer active (which will stay active until the robot has stopped).

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 620 to 633 in 00202b4

	bool TrackingPidLocalPlanner::cancel()
	{
	// This function runs in a separate thread
	cancel_requested_ = true;
	cancel_in_progress_ = true;
	ros::Rate r(10);
	ROS_INFO("Cancel requested, waiting in loop for cancel to finish");
	while (active_goal_)
	{
	r.sleep();
	}
	ROS_INFO("Finished waiting loop, done cancelling");
	return true;
	}

This is not according to design, not sure if this causes any trouble with canceling, but it might. Instead, it should just return true immediately because you implemented a stopping behavior, like described here:
https://github.com/magazino/move_base_flex/blob/a5c19cb8f3135679e4f8f36a9ec6123b121d69ab/mbf_abstract_nav/src/abstract_controller_execution.cpp#L257-L259

7

This single function is close to 470 lines of code. (Makes you wonder if this is really just a PID controller...) We should at least split this up for readability:

path_tracking_pid/src/controller.cpp

Line 399 in 00202b4

geometry_msgs::Twist Controller::update(const double target_x_vel,

To be continued...

Angle calculation of closestPointOnSegment() should be changed

Currently, closestPointOnSegment uses the angle of the start pose (or the one defined from start to end) to the returned Pose

A better idea is to interpolate the angle from start to end (when those angles are given). This will avoid "step" discontinuities of the returned Pose angle between segments, which I saw recently in one of our projects.

Controller inherits from `nav_core::BaseLocalPlanner`, but does not support it

The controller as a child of nav_core::BaseLocalPlanner has not been tested for a long time. It has been decided to drop this support (see #89 ), so we should remove the inheritance here:

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 22 to 25 in 61c6cc8

	// register planner as move_base and move_base plugins
	PLUGINLIB_EXPORT_CLASS(path_tracking_pid::TrackingPidLocalPlanner, nav_core::BaseLocalPlanner)
	PLUGINLIB_EXPORT_CLASS(
	path_tracking_pid::TrackingPidLocalPlanner, mbf_costmap_core::CostmapController)

`Controller::mpc_based_max_vel()` and `Controller::update()` interaction should be refactored

Controller::mpc_based_max_vel() repeatedly calls Controller::update() but rolls back some of its side effects. This is a very convoluted and error-prone setup.

update() sets the following member variables:

• current_with_carrot_
• current_pos_on_plan_
• current_goal_
• distance_to_goal_ by findPositionOnPlan() - see #123
• controller_state_
• current_target_x_vel_

Before mpc_based_max_vel() calls update() it creates a backup of controller_state_ and at certain points in its body (and just before the end) it overwrites the current state with this backup.

I propose the following:

• Ideally mpc_based_max_vel() should be a const function that does not modify any state. Based on the name and function documentation, this should be possible.
• If that's not feasible, update() should be refactored/split in such a way that at the very least no side effects need to be rolled back in mpc_based_max_vel().

Collision polygon is rather conservative

The polygon (in red) encapsulating all predicted footprints (blue) is rather conservative, to the point where a feasible global plan cannot be traversed due to collision avoidance. See example below:

Use only filtered error in P I D calculations

Currently the filtered error is used in the P and D calculations but the raw error is used in the I calculation. All of them should use the filtered one.

Global plan is statically transformed to the global frame

In TrackingPidLocalPlanner::setPlan() the passed global plan is transformed just once into the configured global_frame.
If this latter is configured to be odom, the global plan is sensitive to initial localisation faults.

Other planners (such as dwa_local_planner) convert the inserted global plan every tick into global_frame, using LocalPlannerUtil::getLocalPlan().

Some variable names are unclear

There is a number of variables that represent dimensions and quantities that are referenced in the explanation of the control strategy in the README. Naming of these variables is not consistent, though. Some variables are just unclear in what they represent from their names. This holds for (but is not limited to):

• l: the lookahead distance (not mentioned as such in the README yet)
• current_tf
• some of the controller state members, such as current_x_vel (which is the setpoint, I suppose)

Step response of the lowpass filter has an overshoot

I put a step response on the second order lowpass and the output I've printed below. As you can see the filter has an overshoot, which is very strange. I think there is something wrong with it.

0.292893
0.87868
1.12132
1.02082
0.979185
0.996429
1.00357
1.00061
0.999387
0.999895
1.00011
1.00002
0.999982
0.999997
1
1
0.999999
1
1
1

Calling cancel blocks the controller

A call to cancel should return whether or not the controller implements stopping behavior. If false, the base class handles it (and outputs a zero velocity setpoint right away). Meaning we should not block, but return true immediately, in this case.

Source: https://github.com/magazino/move_base_flex/blob/a5c19cb8f3135679e4f8f36a9ec6123b121d69ab/mbf_abstract_nav/src/abstract_controller_execution.cpp#L257-L259

Tests don't fail while tracking error is ginormous.

The tests only check for the final tracking error, not the maximum or at some point where it matters most. The actual tracking error in many tests looks like in the image below. The error is in the order of magnitude of a meter, and these tests now happily succeed.

Check if constants should be replaced by user-configurable parameters

In a few places, there's magic numbers in the code. We may want to get rid of those. Some examples:

path_tracking_pid/include/path_tracking_pid/controller.hpp

Lines 22 to 24 in 1495813

	#define RADIUS_EPS 0.001 // Smallest relevant radius [m]
	#define VELOCITY_EPS 1e-3 // Neglegible velocity
	#define LONG_DURATION 31556926 // A year (ros::Duration cannot be inf)

path_tracking_pid/include/path_tracking_pid/path_tracking_pid_local_planner.hpp

Lines 24 to 25 in 1495813

	#define MAP_PARALLEL_THRESH 0.2
	constexpr double DT_MAX=1.5;

path_tracking_pid/include/path_tracking_pid/controller.hpp

Lines 330 to 338 in 1495813

	// Upper and lower saturation limits
	double upper_limit_ = 100.0;
	double lower_limit_ = -100.0;
	double ang_upper_limit_ = 100.0;
	double ang_lower_limit_ = -100.0;
	// Anti-windup term. Limits the absolute value of the integral term.
	double windup_limit_ = 1000.0;

Current logic to slow down vehicles too conservative for robots with very asymmetric footprint

Currently the robot slows down based on values provided by the costmap2d. However, costmap2d fills its values based on a logic of robots with a (very close to) circular footprint robots. This makes that for rectangular robots, the robots would slow down a lot more when passing next to an obstacle than when it is approached with the corner. This can hinder the performance in environments with a lot of objects around like warehouses.

Possible unsigned underflow

PR #25 changed this line:

path_tracking_pid/src/controller.cpp

Line 122 in b5c1391

for (std::vector<geometry_msgs::PoseStamped>::size_type pose_idx = 1; pose_idx < global_plan.size() - 1; ++pose_idx)

@lewie-donckers I think this is quite dangerous. If pose_idx is an unsigned int, that means that this comparison can underflow when global plan is empty.

Originally posted by @Rayman in #25 (comment)

`Controller::getControllerState()` should be refactored

This function breaks the abstraction and provides way too much detail to the caller. Only a few of its members are actually used by TrackingPidLocalPlanner. The Controller should provide dedicated getters with a higher level of abstraction and proper names.

This will be a lot easier once #123 has been done so the entire state structure does not need to be passed to that function.

`path_tracking_pid::PidConfig config_` should be decoupled from dynamic reconfigure

dynamic reconfigure is an implementation of ROS which happens to use a large struct for its parameters.

However some of these we don't like to have dynamically reconfigurable.

I propose to have an own struct with the parameters. And let the path_tracking_pid::PidConfig config be local to the reconfigure callback. In this callback we can (if approved) set the controller parameters with setters.

This might allow to switch to ddynamic_reconfigure (if we want to). Also allows for easier ROS2 transition in the future (it uses a different method for dynamic reconfigure behavior).

how to use full_coverage_path_planner and path_tracking_pid?

Thank for sharing u code. When I use full_coverage_path_planner and path_tracking_pid, I can not make it run. please help me. thanks.

this is the launch:

<!--Move base flex, using the full_coverage_path_planner-->
<node pkg="mbf_costmap_nav" type="mbf_costmap_nav" respawn="false" name="move_base_flex" output="screen" required="true">
    <param name="tf_timeout" value="1.5"/>
    <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/planners.yaml" command="load" />
    <rosparam file="$(find full_coverage_path_planner)/test/full_coverage_path_planner/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find path_tracking_pid)/param/controllers.yaml" command="load" />
    <rosparam file="$(find path_tracking_pid)/param/path_tracking_pid.yaml" command="load" />
    <param name="SpiralSTC/robot_radius" value="$(arg robot_radius)"/>
    <param name="SpiralSTC/tool_radius" value="$(arg tool_radius)"/>
    <param name="global_costmap/robot_radius" value="$(arg robot_radius)"/>
    <remap from="odom" to="/odom"/>
    <remap from="scan" to="/scan"/>

    <remap from="/move_base_flex/SpiralSTC/plan" to="/move_base/SpiralSTC/plan"/>
    <!-- <remap from="/move_base_flex/PathTrackingPID/interpolator" to="/move_base/TrackingPidLocalPlanner/interpolator"/> -->
</node>

<!-- Move Base backwards compatibility -->
<node pkg="mbf_costmap_nav" type="move_base_legacy_relay.py" name="move_base" >
    <param name="base_global_planner" value="SpiralSTC" />
    <param name="base_local_planner" value="PathTrackingPID" />
</node>

<!-- Mobile robot simulator -->
<node pkg="mobile_robot_simulator" type="mobile_robot_simulator_node" name="mobile_robot_simulator" output="screen">
    <param name="publish_map_transform" value="true"/>
    <param name="publish_rate" value="10.0"/>
    <param name="velocity_topic" value="/move_base/cmd_vel"/>
    <param name="odometry_topic" value="/odom"/>
</node>

<!--We need a map to fully cover-->
<node name="grid_server" pkg="map_server" type="map_server" args="$(arg map)">
    <param name="frame_id" value="map"/>
</node>

<!-- Launch coverage progress tracking -->
<node pkg="tf" type="static_transform_publisher" name="map_to_coveragemap" args="$(arg coverage_area_offset) map coverage_map 100" />
<node pkg="full_coverage_path_planner" type="coverage_progress" name="coverage_progress">
    <param name="~target_area/x" value="$(arg coverage_area_size_x)" />
    <param name="~target_area/y" value="$(arg coverage_area_size_y)" />
    <param name="~coverage_radius" value="$(arg tool_radius)" />
    <remap from="reset" to="coverage_progress/reset" />
    <param name="~map_frame" value="/coverage_map"/>
</node>

<!-- Trigger planner by publishing a move_base goal -->
<node name="publish_simple_goal" pkg="rostopic" type="rostopic"  launch-prefix="bash -c 'sleep 1.0; $0 $@' "
    args="pub --latch /move_base/goal move_base_msgs/MoveBaseActionGoal --file=$(find full_coverage_path_planner)/test/simple_goal.yaml"/>

<!-- rviz -->
<node if="$(eval rviz)" name="rviz" pkg="rviz" type="rviz" args="-d $(find full_coverage_path_planner)/test/full_coverage_path_planner/fcpp.rviz" />

Documentation of `Controller::distToSegmentSquared()` incorrect

While working on unit tests for Controller::distToSegmentSquared() I noticed the following.

The documentation suggests that the pose it returns is the point on the segment PV (start, end) that is closest to W (point). It does not do this.

I had the following test case:

TEST(PathTrackingPidCalculations, ClosestPointOnSegment_CloseToStart)
{
  const auto start = create_transform(2, 2, 0);
  const auto end = create_transform(4, 4, 0);
  const auto point = create_transform(0, 1, 0);
  const auto ref = start;

  EXPECT_EQ(ref, closestPointOnSegment(start, end, point, false));
}

point is closest to start of any point on start-end but the following is returned: [1.759, 2.319, 0].

Controller uses path poses differently than expected

In determining the error, the controller assumes that the plan is "forward". I.e., the poses' x-axes are aligned with the direction of motion of the robot. It seems that the orientations are assumed to point towards the intended direction of motion. Using that assumption, you can use the poses on the plan to determine a path for the carrot to make the robot follow the plan well. To me, this seems like an abuse of the interface.

I've always interpreted these orientations as description of the robot's prescribed orientation at that position; not its direction of motion (although I can't find a strict definition of this). So the poses' may for example be in opposite direction, indicating that the robot should drive in reverse. Or they could be turned pi/2 in any direction, indicating that a (holonomic?) robot should traverse these points in a sideways motion, see the following video:

Using that definition, the poses may be oriented in any arbitrary direction. At multiple points in the code, this assumption throws a spanner in the works when sending goals that don't follow this assumption. Try sending a goal with interpolated poses (using ROS' default Global Planner for example)...

Does `Controller::findPositionOnPlan()` do what is intended?

While refactoring Controller::findPositionOnPlan(), I noticed the following. I'm not sure if it is intended or not so I would like some feedback. Depending on the feedback we can determine what to do (nothing, document, fix, etc).

In that function we look ahead in the plan based on a given index. As long as each next point is a better match, we keep looking and update the given index. Once we're done with looking ahead, we start looking backward. Now one of two things will happen:

• either we found no better match when looking forward, and we start looking backward as expected
• we did find a better match when looking forward and - because we updated the given index (= starting point) - looking backward from this index will immediately fail.

So in practice we look either forward or backward but never both. Is this intended?

I've included the code below and stripped the bits that were less relevant:

  // First look in current position and in front
  for (auto idx_path = global_plan_index; idx_path < global_plan_tf_.size(); idx_path++) {
    ...
    if (...) {
      ...
      global_plan_index = idx_path;
    } else {
      break;
    }
  }

  // Then look backwards
  for (auto idx_path = global_plan_index; idx_path > 0; --idx_path) {
    ...
  }

Controller reconfigures itself on cancel

When cancelling the exe_path action, the controller changes the configuration set by the user: it sets target_x_vel to zero, and it never changes it back. As a result, the next goal sent on the exe_path action interface is not executed well, because the target_x_vel remains zero.

Slow down based on expanded-footprint <=> lethal-obstacle distance

Controller does not support `nav_core::BaseLocalPlanner`

Original move_base's nav_core::BaseLocalPlanner is inherited from, but not properly supported.

`computeVelocityCommand` has nested function with the same name.

This whole function only existed for the sake of the nav_core implementation.

Now that we don't have that legacy anymore. I think there are much better improvements to be made.

No we're calling computeVelocityCommands from the computeVelocityCommands function. It doesn't add any semantics. The 'main' computeVelocityCommands could be broken into functions with better descriptions.

Originally posted by @Timple in #115 (comment)

Move more visualization away from implementation

Maybe we could also move all of this to visualization.[h|c]pp

path_tracking_pid/src/path_tracking_pid_local_planner.cpp

Lines 295 to 359 in 1495813

	// configure rviz visualization
	visualization_msgs::Marker mkSteps;
	mkSteps.header.frame_id = map_frame_;
	mkSteps.header.stamp = ros::Time::now();
	mkSteps.ns = "extrapolated poses";
	mkSteps.action = visualization_msgs::Marker::ADD;
	mkSteps.pose.orientation.w = 1.0;
	mkSteps.id = __COUNTER__;
	mkSteps.type = visualization_msgs::Marker::POINTS;
	mkSteps.scale.x = 0.5;
	mkSteps.scale.y = 0.5;
	mkSteps.color.r = 1.0;
	mkSteps.color.g = 0.5;
	mkSteps.color.a = 1.0;
	visualization_msgs::Marker mkPosesOnPath;
	mkPosesOnPath.header.frame_id = map_frame_;
	mkPosesOnPath.header.stamp = ros::Time::now();
	mkPosesOnPath.ns = "goal poses on path";
	mkPosesOnPath.action = visualization_msgs::Marker::ADD;
	mkPosesOnPath.pose.orientation.w = 1.0;
	mkPosesOnPath.id = __COUNTER__;
	mkPosesOnPath.type = visualization_msgs::Marker::POINTS;
	mkPosesOnPath.scale.x = 0.5;
	mkPosesOnPath.scale.y = 0.5;
	mkPosesOnPath.color.r = 1.0;
	mkPosesOnPath.color.g = 1.0;
	mkPosesOnPath.color.a = 1.0;
	visualization_msgs::Marker mkCollisionFootprint;
	mkCollisionFootprint.header.frame_id = map_frame_;
	mkCollisionFootprint.header.stamp = ros::Time::now();
	mkCollisionFootprint.ns = "Collision footprint";
	mkCollisionFootprint.action = visualization_msgs::Marker::ADD;
	mkCollisionFootprint.pose.orientation.w = 1.0;
	mkCollisionFootprint.id = __COUNTER__;
	mkCollisionFootprint.type = visualization_msgs::Marker::LINE_LIST;
	mkCollisionFootprint.scale.x = 0.1;
	mkCollisionFootprint.color.b = 1.0;
	mkCollisionFootprint.color.a = 0.3;
	visualization_msgs::Marker mkCollisionHull;
	mkCollisionHull.header.frame_id = map_frame_;
	mkCollisionHull.header.stamp = ros::Time::now();
	mkCollisionHull.ns = "Collision polygon";
	mkCollisionHull.action = visualization_msgs::Marker::ADD;
	mkCollisionHull.pose.orientation.w = 1.0;
	mkCollisionHull.id = __COUNTER__;
	mkCollisionHull.type = visualization_msgs::Marker::LINE_STRIP;
	mkCollisionHull.scale.x = 0.2;
	mkCollisionHull.color.r = 1.0;
	mkCollisionHull.color.a = 0.3;
	visualization_msgs::Marker mkCollisionIndicator;
	mkCollisionIndicator.header.frame_id = map_frame_;
	mkCollisionIndicator.header.stamp = ros::Time::now();
	mkCollisionIndicator.ns = "Collision object";
	mkCollisionIndicator.pose.orientation.w = 1.0;
	mkCollisionIndicator.id = __COUNTER__;
	mkCollisionIndicator.type = visualization_msgs::Marker::CYLINDER;
	mkCollisionIndicator.scale.x = 0.5;
	mkCollisionIndicator.scale.y = 0.5;
	mkCollisionIndicator.color.r = 1.0;
	mkCollisionIndicator.color.a = 0.0;
	visualization_msgs::MarkerArray mkCollision;

Is the calculated stopping distance too conservative?

The stopping distance in the end phase d_end_phase (here) contains a component which accounts for the distance traveled during the possible delay due to having a sampling time.

However I wonder if it isn't too conservative, currently it is:

target_x_vel * 2 * dt

Whereas I think the average delay is 0.5*dt, which would result in the following component:

target_x_vel * 0.5 * dt

Controller never succeeds, and turns around at the end of the path

I'm sending a plan produced by the generic global_planner/GlobalPlanner, and this happens:

After this, it slowly follows the path back to the start.

Tests broken

This is not a proper forward prediction of the vehicle footprint.

Test probably 'succeeds' because of poor criteria.

about how use this package

Hi thanks for your work.
can you please give a example about how to use this package with global planner, I cannot understand how to subscribe topic "path". I readed the code , it do not subscribe any topic namde "path" or any topic with type of nav_msgs/Path.

thank you very much

How to run with the branch "first-ros2-conversion"

Since there is no launch file nor test script, how can I run the path tracker?
I can build it successfully with colcon build.

Thanks!

Simplify the math being done to calculate direction to goal

A simple dot product would give the desired outcome: the length of the projection of one vector onto the other, with the sign of the cosine of the angle between the two vectors (which is what we're looking for).

path_tracking_pid/src/controller.cpp

Lines 581 to 593 in 61c6cc8

	// Get 'angle' towards current_goal
	tf2::Transform robot_pose;
	tf2::convert(current_tf, robot_pose);
	tf2::Transform base_to_goal = robot_pose.inverseTimes(current_goal_);
	const double angle_to_goal = atan2(base_to_goal.getOrigin().x(), -base_to_goal.getOrigin().y());
	// If we are as close to our goal or closer then we need to reach end velocity, enable end_phase.
	// However, if robot is not facing to the same direction as the local velocity target vector, don't enable end_phase.
	// This is to avoid skipping paths that start with opposite velocity.
	if ((distance_to_goal_ <= fabs(d_end_phase)) && have_same_sign(target_x_vel, angle_to_goal)) {
	// This state will be remebered to avoid jittering on target_x_vel
	controller_state_.end_phase_enabled = true;
	}

UB in `TrackingPidLocalPlanner::projectedCollisionCost()` - partially uninitialized object used

In TrackingPidLocalPlanner::projectedCollisionCost() a local variable x_step_tf is created but only it's origin component is initialized; it's basis component is left uninitialized.

Later this variable is used in computations which is undefined behavior.

This could be (one of) the reasons our tests sometimes fail.

About the use of path_tracking_pid function package

@Timple
Hello，I put the path_tracking_pid package in the workspace for compilation, and found whether nav_core is registered, and the result is as follows:

At the same time, my launch file configuration is as follows:

But when I run it, the following error appears:

How can I solve this problem? Looking forward to your reply！

`Controller::distToSegmentSquared()` should be refactored

I think Controller::distToSegmentSquared() should be refactored in several ways:

1. Its name suggests it calculates a distance but the main purpose is to determine a pose: the closest pose on segment. I propose we change the name to reflect this.
2. Beside the pose, it also returns distances but these can easily be calculated by the caller (if required). I propose we remove those distances from the output.
3. It's currently a const member function but it only uses the data member estimate_pose_angle_enabled_. I propose we replace that with an input parameter.
4. If the previous point has been done, the function can be extracted (to calculations.h/cpp for example) and unit tests can be added.
5. To do this, distSquared() from the anonymous namespace in controller.cpp would have to be moved too. I propose we do this and add unit tests for that as well.

`Controller::findPositionOnPlan()` should be refactored

Controller::findPositionOnPlan() should be refactored:

1. Its name suggests it only finds a position but it also sets member variable distance_to_goal_. It is incorrect (or at least odd) to only change the distance but not any other state (current plan index, etc). I propose we make this function const and do not set any data members. It should just do as the name suggests and find something. If required this value can be made part of the return value. (But perhaps it might be easy to calculate by the caller.)
2. Parameter path_pose_idx is an output parameter. I propose we change it to be part of the return value.
3. Parameter controller_state contains way too much state. Only the following members are used:
   • last_visited_pose_index is used as an output parameter. I propose we change it to be part of the return value.
   • current_global_plan_index is used as an in-/output parameter. I propose we split this into a pure input parameter and make the output part of the return value.

Regarding the inability to trace the global path？

Hello, I am using this package alone to follow the global path planned by myself, but the effect is very poor, mainly as follows:

1. The robot can only follow in a straight line, and it will deviate greatly when turning, and it will not even be able to track until the path will eventually stop.
2. The robot will stop when it approaches an obstacle, but when there is a new path, the robot will still stop and cannot move unless the robot is repositioned.
   The following are the parameters I configured:

controllers.yaml

controllers:

• name: PathTrackingPID
  type: path_tracking_pid/TrackingPidLocalPlanner
  PathTrackingPID:
  holonomic_robot: false
  track_base_link: true
  estimate_pose_angle: false
  base_link_frame: base_link
  Kd_ang: 0.3
  Kd_lat: 0.3
  Ki_ang: 0.0
  Ki_lat: 0.0
  Kp_ang: 1.0
  Kp_lat: 1.0
  l: 1.0
  feedback_ang: false
  feedback_lat: true
  feedforward_ang: false
  feedforward_lat: true
  controller_debug_enabled: true
  target_end_x_vel: 0.0
  target_x_acc: 2.0
  target_x_decc: 2.0
  target_x_vel: 2.0
  abs_minimum_x_vel: 0.0
  max_error_x_vel: 1.0
  max_yaw_vel: 2.0
  max_yaw_acc: 2.0
  min_turning_radius: 0.0

use_mpc: false
mpc_max_error_lat: 0.5
mpc_max_fwd_iter: 200
mpc_max_vel_optimization_iterations: 5
mpc_min_x_vel: 0.5
mpc_simulation_sample_time: 0.05

anti_collision: true
obstacle_speed_reduction: true
collision_look_ahead_resolution: 1.0 # [m]

global_frame_id: odom
map_frame: map

controller_frequency: 5.0
controller_max_retries: 0

publishing nan velocity due to infinite inverse radius assignment

Came across this corner case where angular field in the command velocity is set to nan which may cause erratic movements on a robot if not handled on the receiving side. The nan value is coming from feedforward_ang_ component of the controller, which uses inverse turning radius as:

path_tracking_pid/src/controller.cpp

Line 559 in 750f8c1

turning_radius_inv_vector_[find_result.last_visited_pose_index] * control_effort_long_;

The part where inverse turn radii are computed have a return value of infinity when delta is below an epsilon specified which includes a zero value too :

path_tracking_pid/src/calculations.cpp

Lines 45 to 60 in 750f8c1

	std::vector<double> inverse_turning_radiuses(const std::vector<tf2::Transform> & deltas)
	{
	auto result = std::vector<double>{};
	result.reserve(deltas.size() + 1);
	std::transform(
	deltas.cbegin(), deltas.cend(), std::back_inserter(result), [](const tf2::Transform & d) {
	const auto & origin = d.getOrigin();
	const auto dpX = origin.x();
	const auto dpY = origin.y();
	const auto dpXY2 = std::pow(dpX, 2) + std::pow(dpY, 2);
	if (dpXY2 < FLT_EPSILON) {
	return std::numeric_limits<double>::infinity();
	}
	return (2 * dpY) / dpXY2;
	});

This is causing an instance of nan command velocity (angular) in the controller which stays permanent even if inverse radii return back to finite values.

`Controller::distToSegmentSquared()` ignores z-component half the time

While working on unit tests for Controller::distToSegmentSquared() I noticed that the z-component of the inputs is ignored in some calculations but not in all.

I doubt that this is intended; it looks like a bug but perhaps someone can shed some light on this:

  const double l2 = distSquared(pose_v, pose_w); // z used for l2
  if (l2 == 0) {
    return pose_w; // <-- z included in result in this case. not in the other return
  }

  tf2::Transform result;

  const double t = std::clamp(
    ((pose_p.getOrigin().x() - pose_v.getOrigin().x()) *
       (pose_w.getOrigin().x() - pose_v.getOrigin().x()) +
     (pose_p.getOrigin().y() - pose_v.getOrigin().y()) *
       (pose_w.getOrigin().y() - pose_v.getOrigin().y())) /
      l2,
    0.0, 1.0); // <-- z used in l2 but not in rest of calculation of t. which in turn is used for x and y of result.
  result.setOrigin(tf2::Vector3(
    pose_v.getOrigin().x() + t * (pose_w.getOrigin().x() - pose_v.getOrigin().x()),
    pose_v.getOrigin().y() + t * (pose_w.getOrigin().y() - pose_v.getOrigin().y()), 0.0)); // <-- z forced to 0 in result

  const auto yaw = estimate_pose_angle ? atan2(
                                           pose_w.getOrigin().y() - pose_v.getOrigin().y(),
                                           pose_w.getOrigin().x() - pose_v.getOrigin().x())
                                       : tf2::getYaw(pose_v.getRotation());
  tf2::Quaternion pose_quaternion;
  pose_quaternion.setRPY(0.0, 0.0, yaw);
  result.setRotation(pose_quaternion);

  return result;

Straight line into turn: starting turn too early

After a discussion with @cesar-lopez-mar:

In the figure below,

• a straight path is followed by a corner
• the robot starts executing the straight path, as indicated by the red arrows
• the debugging markers of the path_tracking_pid are indicated as well
• halfway, the robot starts rotating. Based on the debug markers, it seems that starts executing the corners.

A cancel of the `exe_path` action sets `target_x_vel` parameter to zero

Collision polygon does not include all predicted footprints entirely

Blue is the predicted footprint, red is the collision polygon. The latter should circumscribe all blue polygons.

Controller is too conservative in its collision checking

The controller checks if there is any cell within the collision polygon that is of value costmap2d::INSCRIBED_INFLATED_OBASTACLE. If there is, it concludes that a collision is imminent. This is not true. the robot footprint is perfectly allowed to enter space of costmap2d::INSCRIBED_INFLATED_OBASTACLE value. Here's an example where there is overlap, but no collision:

The cheap way to check for imminent collisions, assuming a (approximately) circular base is to check if the cell in which (a predicted) base_link is located is >= costmap2d::INSCRIBED_INFLATED_OBASTACLE. That would mean a guaranteed collision.

However, with a non-circular base, this is not enough. Because a different orientation may also lead to collision before base_link reaches this space. Instead, you can check if any of the cells in the collision polygon exceeds this value. That means it reaches a value of >= costmap2d::LETHAL_OBSTACLE.