Code review

[rokusottervanger]

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...

[Timple]

Thanks for the review!

There's an open PR for facilitate such a review: #2
Might be easier to paste the comments there so the threads stick together

[rokusottervanger]

Moving this discussion to #2