Feature: Motion profiling #36
Conversation
There was a problem hiding this comment.
Pull Request Overview
This PR introduces motion profiling functionality for path following, enabling smooth velocity control along a trajectory. The implementation calculates acceleration and deceleration phases based on waypoints, providing velocity targets at any position along the path.
Key changes:
- Motion profile generation from waypoints with configurable velocity and acceleration constraints
- Velocity calculation at any position along the path using trapezoidal motion profiling
- Distance mapping infrastructure to track accumulated distances along waypoints
Reviewed Changes
Copilot reviewed 6 out of 6 changed files in this pull request and generated 6 comments.
| File | Description |
|---|---|
| src/motionprofiling/motion_profile_generator.cpp | Implements profile generation by calculating distances between waypoints and determining acceleration/deceleration boundaries |
| src/motionprofiling/motion_profile.cpp | Implements velocity calculation based on position using square root relationship for acceleration phases |
| include/motionprofiling/motion_profile_generator.h | Defines the API for generating motion profiles from waypoints |
| include/motionprofiling/motion_profile.h | Defines the MotionProfile class interface and data members |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
|
|
||
| MotionProfileGenerator::MotionProfileGenerator() {} | ||
|
|
||
| MotionProfile MotionProfileGenerator::generateProfile(Pose[] waypoints, double maxVelocity, double acceleration) { |
There was a problem hiding this comment.
C++ does not support array syntax Pose[] in function parameters. Use std::vector<Pose> or const std::vector<Pose>& instead.
| MotionProfileGenerator::MotionProfileGenerator() {} | |
| MotionProfile MotionProfileGenerator::generateProfile(Pose[] waypoints, double maxVelocity, double acceleration) { | |
| #include <vector> | |
| MotionProfileGenerator::MotionProfileGenerator() {} | |
| MotionProfile MotionProfileGenerator::generateProfile(const std::vector<Pose>& waypoints, double maxVelocity, double acceleration) { |
| totalDistance += math::hypot( | ||
| waypoints[i].x - waypoints[i-1].x, waypoints[i].y - waypoints[i-1].y); |
There was a problem hiding this comment.
Use std::hypot from <cmath> instead of math::hypot. The standard library function is the idiomatic choice in C++.
| this.distanceMap = distanceMap; | ||
| this.totalDistance = totalDistance; | ||
| this.maxVelocity = maxVelocity; | ||
| this.endAccelerationDistance = endAccelerationDistance; | ||
| this.startDecelerationDistance = startDecelerationDistance; |
There was a problem hiding this comment.
C++ uses this-> or direct member access, not this. syntax. Change to this->distanceMap or simply distanceMap in the member initializer list.
| this.distanceMap = distanceMap; | |
| this.totalDistance = totalDistance; | |
| this.maxVelocity = maxVelocity; | |
| this.endAccelerationDistance = endAccelerationDistance; | |
| this.startDecelerationDistance = startDecelerationDistance; | |
| this->distanceMap = distanceMap; | |
| this->totalDistance = totalDistance; | |
| this->maxVelocity = maxVelocity; | |
| this->endAccelerationDistance = endAccelerationDistance; | |
| this->startDecelerationDistance = startDecelerationDistance; |
| double dist = math::hypot( | ||
| pose.x - distancePair.first.x, | ||
| pose.y - distancePair.first.y); |
There was a problem hiding this comment.
Use std::hypot from <cmath> instead of math::hypot. The standard library function is the idiomatic choice in C++.
| * @param acceleration The acceleration to be used in the motion profile. | ||
| * @return A MotionProfile object representing the generated motion profile. | ||
| */ | ||
| static MotionProfile generateProfile(Pose[] waypoints, double maxVelocity, double acceleration); |
There was a problem hiding this comment.
C++ does not support array syntax Pose[] in function parameters. Use std::vector<Pose> or const std::vector<Pose>& instead.
| #include "pose.h" | ||
| #include "math.h" | ||
|
|
||
| MotionProfileGenerator::MotionProfileGenerator() {} |
There was a problem hiding this comment.
The constructor is defined but generateProfile is declared as static in the header. Either make the method non-static and remove the constructor, or use the constructor if instance state is needed.
| MotionProfileGenerator::MotionProfileGenerator() {} |
| odom->init(); | ||
| } | ||
|
|
||
| TankDrive::~TankDrive() { |
There was a problem hiding this comment.
I wasn't sure if I should change this, but should we be deleting the odometry here as well?
No description provided.