Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Refactored output parameter. #75

Closed
wants to merge 1 commit into from
Closed

Refactored output parameter. #75

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 11 additions & 12 deletions include/path_tracking_pid/controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,23 +91,22 @@ class Controller : private boost::noncopyable
const geometry_msgs::Transform & tf_base_to_steered_wheel,
const geometry_msgs::Twist & steering_odom_twist,
const std::vector<geometry_msgs::PoseStamped> & global_plan);

// Result of findPositionOnPlan().
struct FindPositionResult
{
tf2::Transform position;
std::size_t path_pose_index = 0;
};

/**
* Find position on plan by looking at the surroundings of last known pose.
* @param current Where is the robot now?
* @param controller_state_ptr The current state of the controller that gets updated by this function
* @return tf of found position on plan
* @return index of current path-pose if requested
* @return tf of found position on plan and the index of current path-pose
*/
tf2::Transform findPositionOnPlan(
const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr,
size_t & path_pose_idx);
// Overloaded function definition for users that don't require the segment index
tf2::Transform findPositionOnPlan(
const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr)
{
size_t path_pose_idx;
return findPositionOnPlan(current_tf, controller_state_ptr, path_pose_idx);
}
FindPositionResult findPositionOnPlan(
const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr);

/**
* Run one iteration of a PID controller
Expand Down
50 changes: 27 additions & 23 deletions src/controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -319,9 +319,8 @@ void Controller::distToSegmentSquared(
}
}

tf2::Transform Controller::findPositionOnPlan(
const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr,
size_t & path_pose_idx)
Controller::FindPositionResult Controller::findPositionOnPlan(
const geometry_msgs::Transform & current_tf, ControllerState * controller_state_ptr)
{
tf2::Transform current_tf2;
tf2::convert(current_tf, current_tf2);
Expand Down Expand Up @@ -376,8 +375,8 @@ tf2::Transform Controller::findPositionOnPlan(
global_plan_tf_.size() - 1);
// To finalize, compute the indexes of the start and end points of
// the closest line segment to the current carrot
tf2::Transform current_goal_local;
current_goal_local = global_plan_tf_[controller_state_ptr->current_global_plan_index];
FindPositionResult result;
result.position = global_plan_tf_[controller_state_ptr->current_global_plan_index];

tf2::Transform pose_projection_ahead;
tf2::Transform pose_projection_behind;
Expand All @@ -389,19 +388,19 @@ tf2::Transform Controller::findPositionOnPlan(
distToSegmentSquared(
current_tf2, global_plan_tf_[0], global_plan_tf_[1], pose_projection_ahead,
distance2_to_line_ahead, distance_to_end_line_ahead);
current_goal_local = pose_projection_ahead;
result.position = pose_projection_ahead;
distance_to_goal_ = distance_to_goal_vector_[1] + distance_to_end_line_ahead;
controller_state_ptr->last_visited_pose_index = 0;
path_pose_idx = controller_state_ptr->current_global_plan_index;
result.path_pose_index = controller_state_ptr->current_global_plan_index;
} else if (controller_state_ptr->current_global_plan_index == global_plan_tf_.size() - 1) {
distToSegmentSquared(
current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index - 1],
global_plan_tf_[controller_state_ptr->current_global_plan_index], pose_projection_behind,
distance2_to_line_behind, distance_to_end_line_behind);
current_goal_local = pose_projection_behind;
result.position = pose_projection_behind;
distance_to_goal_ = distance_to_end_line_behind;
controller_state_ptr->last_visited_pose_index = global_plan_tf_.size() - 2;
path_pose_idx = controller_state_ptr->current_global_plan_index - 1;
result.path_pose_index = controller_state_ptr->current_global_plan_index - 1;
} else {
distToSegmentSquared(
current_tf2, global_plan_tf_[controller_state_ptr->current_global_plan_index],
Expand All @@ -413,23 +412,23 @@ tf2::Transform Controller::findPositionOnPlan(
distance2_to_line_behind, distance_to_end_line_behind);

if (distance2_to_line_ahead < distance2_to_line_behind) {
current_goal_local = pose_projection_ahead;
result.position = pose_projection_ahead;
distance_to_goal_ =
distance_to_goal_vector_[controller_state_ptr->current_global_plan_index + 1] +
distance_to_end_line_ahead;
controller_state_ptr->last_visited_pose_index =
controller_state_ptr->current_global_plan_index;
} else {
current_goal_local = pose_projection_behind;
result.position = pose_projection_behind;
distance_to_goal_ =
distance_to_goal_vector_[controller_state_ptr->current_global_plan_index] +
distance_to_end_line_behind;
controller_state_ptr->last_visited_pose_index =
controller_state_ptr->current_global_plan_index - 1;
}
path_pose_idx = controller_state_ptr->current_global_plan_index;
result.path_pose_index = controller_state_ptr->current_global_plan_index;
}
return current_goal_local;
return result;
}

geometry_msgs::Twist Controller::update(
Expand All @@ -452,24 +451,29 @@ geometry_msgs::Twist Controller::update(
current_tf.rotation.x, current_tf.rotation.y, current_tf.rotation.z, current_tf.rotation.w);
current_with_carrot_.setRotation(cur_rot);

size_t path_pose_idx;
const auto current_tf_for_find_position = [&current_tf, track_base_link = config_.track_base_link,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These lambda functions don't really increase readability. But I get the point.

Although current_tf_for_find_position sounds like something that could be a simple request function on the class?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually, something is off here.

You're comment suggests a current_tf is requested. But there is also a variable current_tf. We should fix these names first.

Probably one tf points to base_link and the other to the tf that tries to follow the carrot. Right @cesar-lopez-mar ?

Copy link
Contributor Author

@lewie-donckers lewie-donckers Feb 22, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, if I move the lambda to a member function we could add some documentation (that was present in the original implementation).

current_tf is used if config_.track_base_link, else the following is used:

    // find position of current position with projected carrot
    geometry_msgs::Transform current_with_carrot_g;
    tf2::convert(current_with_carrot_, current_with_carrot_g);

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These lambda functions don't really increase readability. But I get the point.

Although current_tf_for_find_position sounds like something that could be a simple request function on the class?

I agree that readability does not improve for all developers that might no be familiar with lambda functions (including myself). I would keep the comments of the original implementation in this function to help understanding the new code.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Actually, something is off here.

You're comment suggests a current_tf is requested. But there is also a variable current_tf. We should fix these names first.

Probably one tf points to base_link and the other to the tf that tries to follow the carrot. Right @cesar-lopez-mar ?

Indeed current_tf points to base_link, and the other one to the carrot.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not the carrot. But the ControlPoint.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I opened #87 for this. Referencing here to put it on the list ;)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay, I'm confused as to what the conclusion of this discussion is/should be. I need some help.

Would replacing the lambda with a member function suffice? If so, I would like some help in naming and documenting that function.

Or, is there some underlying problem that needs to be fixed and which is preventing us from continuing with this PR?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, there is an underlying problem that the variable names are off. I propose we fix that first.

I hope a more clear solution direction will follow from that.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These variable names are really confusing indeed. current_tf also seems to imply that this is a tf2::Transform, but looking at this code, that cannot be the case (otherwise this wouldn't compile, as current_with_carrot_g is a geometry_msgs::Transform).

BTW, I like this refactor with the IILE

current_with_carrot = current_with_carrot_]() {
if (track_base_link) {
return current_tf;
}
geometry_msgs::Transform current_with_carrot_g;
tf2::convert(current_with_carrot, current_with_carrot_g);
return current_with_carrot_g;
}();

const auto find_position_result =
findPositionOnPlan(current_tf_for_find_position, &controller_state_);
const auto path_pose_idx = find_position_result.path_pose_index;
current_pos_on_plan_ = current_goal_ = find_position_result.position;

if (config_.track_base_link) {
// Find closes robot position to path and then project carrot on goal
Copy link

@cesar-lopez-mar cesar-lopez-mar Feb 22, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would preserve these kind of comments (after fixing the typo ;)), and move it to the new lambda function

current_pos_on_plan_ = current_goal_ =
findPositionOnPlan(current_tf, &controller_state_, path_pose_idx);
// To track the base link the goal is then transform to the control point goal
double theda_rp = tf2::getYaw(current_goal_.getRotation());
tf2::Vector3 newControlOrigin;
newControlOrigin.setX(current_goal_.getOrigin().x() + config_.l * cos(theda_rp));
newControlOrigin.setY(current_goal_.getOrigin().y() + config_.l * sin(theda_rp));
newControlOrigin.setZ(0);
current_goal_.setOrigin(newControlOrigin);
} else {
// find position of current position with projected carrot
geometry_msgs::Transform current_with_carrot_g;
tf2::convert(current_with_carrot_, current_with_carrot_g);
current_pos_on_plan_ = current_goal_ =
findPositionOnPlan(current_with_carrot_g, &controller_state_, path_pose_idx);
}

*progress = 1.0 - distance_to_goal_ / distance_to_goal_vector_[0];
Expand Down
9 changes: 6 additions & 3 deletions src/path_tracking_pid_local_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -375,12 +375,15 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
tf2::Transform projected_step_tf;
tf2::fromMsg(current_tf, projected_step_tf);
projected_steps_tf.push_back(projected_step_tf); // Evaluate collision at base_link
projected_step_tf = pid_controller_.findPositionOnPlan(current_tf, &projected_controller_state);
projected_step_tf =
pid_controller_.findPositionOnPlan(current_tf, &projected_controller_state).position;
projected_steps_tf.push_back(projected_step_tf); // Add base_link projected pose
for (uint step = 0; step < n_steps; step++) {
tf2::Transform next_straight_step_tf = projected_step_tf * x_step_tf;
projected_step_tf = pid_controller_.findPositionOnPlan(
tf2::toMsg(next_straight_step_tf), &projected_controller_state);
projected_step_tf =
pid_controller_
.findPositionOnPlan(tf2::toMsg(next_straight_step_tf), &projected_controller_state)
.position;
projected_steps_tf.push_back(projected_step_tf);

// Fill markers:
Expand Down