feat: delete_point by providing and index

source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp

@@ -81,9 +81,9 @@ class CartesianTrajectory : public TrajectoryBase<CartesianTrajectoryPoint> {
    * already existing points
    * @param new_point the new trajectory point
    * @param duration the duration for the new point
-   * @param pos the desired position of the new point in the queue
+   * @param index the desired position of the new point in the queue
    */
-  void insert_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos);
+  void insert_point(const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int index);
 
   /**
    * @brief Get list of trajectory points

source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp

@@ -74,9 +74,9 @@ class JointTrajectory : public TrajectoryBase<JointTrajectoryPoint> {
    * already existing points
    * @param new_point the new trajectory point
    * @param duration the duration for the new point
-   * @param pos the desired position of the new point in the queue
+   * @param index the desired position of the new point in the queue
    */
-  void insert_point(const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos);
+  void insert_point(const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int index);
 
   /**
    * @brief Get list of trajectory points

source/state_representation/include/state_representation/trajectory/TrajectoryBase.hpp

@@ -43,14 +43,21 @@ class TrajectoryBase : public State {
   unsigned int get_size() const;
 
   /**
-   * @brief Reset trajectory
+   * @brief Delete the last point from trajectory
    */
-  virtual void reset();
+  void delete_point();
 
   /**
    * @brief Delete the last point from trajectory
+   * @param index the index of the point to delete
+   * @throw std::out_of_range if index is out of range
+   */
+  void delete_point(unsigned int index);
+
+  /**
+   * @brief Reset trajectory
    */
-  virtual void delete_point();
+  virtual void reset();
 
 protected:
   /**
@@ -74,7 +81,7 @@ class TrajectoryBase : public State {
    * @brief Get the trajectory point at given index
    * @param index the index
    * @return the trajectory point
-   * @throw std::out_of_range if pos is out of range
+   * @throw std::out_of_range if index is out of range
    */
   const TrajectoryT& get_point(unsigned int index) const;
 
@@ -93,10 +100,10 @@ class TrajectoryBase : public State {
   /**
    * @brief Insert new trajectory point between two already existing points
    * @param new_point the new point
-   * @param pos the desired position of the new point in the queue
-   * @throw std::out_of_range if pos is out of range
+   * @param index the desired position of the new point in the queue
+   * @throw std::out_of_range if index is out of range
    */
-  void insert_point(const TrajectoryT& new_point, unsigned int pos);
+  void insert_point(const TrajectoryT& new_point, unsigned int index);
 
   /**
    * @brief Get the trajectory point at given index
@@ -174,13 +181,13 @@ inline void TrajectoryBase<TrajectoryT>::add_points(const std::vector<Trajectory
 }
 
 template<typename TrajectoryT>
-inline void TrajectoryBase<TrajectoryT>::insert_point(const TrajectoryT& new_point, unsigned int pos) {
-  if (pos > this->points_.size()) {
+inline void TrajectoryBase<TrajectoryT>::insert_point(const TrajectoryT& new_point, unsigned int index) {
+  if (index > this->points_.size()) {
     throw std::out_of_range("Index out of range");
   }
   this->set_empty(false);
   auto it_points = this->points_.begin();
-  std::advance(it_points, pos);
+  std::advance(it_points, index);
   this->points_.insert(it_points, new_point);
 }
 
@@ -194,6 +201,17 @@ inline void TrajectoryBase<TrajectoryT>::delete_point() {
   }
 }
 
+template<typename TrajectoryT>
+inline void TrajectoryBase<TrajectoryT>::delete_point(unsigned int index) {
+  if (index >= this->points_.size()) {
+    throw std::out_of_range("Index out of range");
+  }
+  this->points_.erase(this->points_.begin() + index);
+  if (this->points_.empty()) {
+    this->set_empty(false);
+  }
+}
+
 template<typename TrajectoryT>
 inline const std::deque<TrajectoryT>& TrajectoryBase<TrajectoryT>::get_points() const {
   return this->points_;

source/state_representation/src/trajectory/CartesianTrajectory.cpp

@@ -72,7 +72,7 @@ void CartesianTrajectory::add_points(
 }
 
 void CartesianTrajectory::insert_point(
-    const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos
+    const CartesianState& new_point, const std::chrono::nanoseconds& duration, unsigned int index
 ) {
   if (new_point.is_empty()) {
     throw exceptions::EmptyStateException("Point is empty");
@@ -91,7 +91,7 @@ void CartesianTrajectory::insert_point(
   trajectory_point.duration = duration;
   trajectory_point.name = new_point.get_name();
   try {
-    this->TrajectoryBase<CartesianTrajectoryPoint>::insert_point(trajectory_point, pos);
+    this->TrajectoryBase<CartesianTrajectoryPoint>::insert_point(trajectory_point, index);
   } catch (...) {
     throw;
   }

source/state_representation/src/trajectory/JointTrajectory.cpp

@@ -63,7 +63,7 @@ void JointTrajectory::add_points(
 }
 
 void JointTrajectory::insert_point(
-    const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int pos
+    const JointState& new_point, const std::chrono::nanoseconds& duration, unsigned int index
 ) {
   if (new_point.is_empty()) {
     throw exceptions::EmptyStateException("Point is empty");
@@ -83,7 +83,7 @@ void JointTrajectory::insert_point(
     JointTrajectoryPoint trajectory_point;
     trajectory_point.data = new_point.data();
     trajectory_point.duration = duration;
-    this->TrajectoryBase<JointTrajectoryPoint>::insert_point(trajectory_point, pos);
+    this->TrajectoryBase<JointTrajectoryPoint>::insert_point(trajectory_point, index);
   } catch (...) {
     throw;
   }