update swap functions in derived classes (requires new getter)

aica-technology · Feb 8, 2025 · 6645d93 · 6645d93
1 parent 79f27c9
commit 6645d93
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 5 deletions.
diff --git a/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/CartesianTrajectory.hpp
@@ -92,12 +92,18 @@ class CartesianTrajectory : public TrajectoryBase<CartesianTrajectoryPoint> {
   const std::string& get_reference_frame() const;
 
   /**
-   * @brief Set the reference frame that applies a transformation to all existing points to change the reference frame
+   * @brief Set the reference frame by applying a transformation to all existing points to change the reference frame
    * @param pose the new pose that needs to be applied to existing points to change the reference frame
    * @throws EmptyStateException if pose is empty
    */
   void set_reference_frame(const CartesianPose& pose);
 
+  /**
+   * @brief Set the reference frame by simply changing the reference frame name
+   * @param reference_frame the new reference frame
+   */
+  void set_reference_frame(const std::string& reference_frame);
+
   /**
    * @brief Get list of trajectory points
    * @return queue of the Cartesian states of the trajectory
@@ -194,7 +200,12 @@ class CartesianTrajectory : public TrajectoryBase<CartesianTrajectoryPoint> {
 };
 
 inline void swap(CartesianTrajectory& trajectory1, CartesianTrajectory& trajectory2) {
-  swap(static_cast<TrajectoryBase<CartesianTrajectoryPoint>&>(trajectory1), static_cast<TrajectoryBase<CartesianTrajectoryPoint>&>(trajectory2));
+  swap(
+      static_cast<TrajectoryBase<CartesianTrajectoryPoint>&>(trajectory1),
+      static_cast<TrajectoryBase<CartesianTrajectoryPoint>&>(trajectory2));
+  auto tmp = trajectory1.get_reference_frame();
+  trajectory1.set_reference_frame(trajectory2.get_reference_frame());
+  trajectory2.set_reference_frame(tmp);
 }
 
 }// namespace state_representation
diff --git a/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp b/source/state_representation/include/state_representation/trajectory/JointTrajectory.hpp
@@ -180,7 +180,12 @@ class JointTrajectory : public TrajectoryBase<JointTrajectoryPoint> {
 };
 
 inline void swap(JointTrajectory& trajectory1, JointTrajectory& trajectory2) {
-  swap(static_cast<TrajectoryBase<JointTrajectoryPoint>&>(trajectory1), static_cast<TrajectoryBase<JointTrajectoryPoint>&>(trajectory2));
+  swap(
+      static_cast<TrajectoryBase<JointTrajectoryPoint>&>(trajectory1),
+      static_cast<TrajectoryBase<JointTrajectoryPoint>&>(trajectory2));
+  auto tmp = trajectory1.get_joint_names();
+  trajectory1.set_joint_names(trajectory2.get_joint_names());
+  trajectory2.set_joint_names(tmp);
 }
 
 }// namespace state_representation
diff --git a/source/state_representation/src/trajectory/CartesianTrajectory.cpp b/source/state_representation/src/trajectory/CartesianTrajectory.cpp
@@ -49,6 +49,10 @@ void CartesianTrajectory::set_reference_frame(const CartesianPose& pose) {
   this->set_points(points, this->get_durations());
 }
 
+void CartesianTrajectory::set_reference_frame(const std::string& reference_frame) {
+  this->reference_frame_ = reference_frame;
+}
+
 const std::vector<CartesianState> CartesianTrajectory::get_points() const {
   std::vector<CartesianState> points;
   auto queue = this->TrajectoryBase<CartesianTrajectoryPoint>::get_points();
@@ -115,7 +119,6 @@ CartesianTrajectory& CartesianTrajectory::operator=(const CartesianTrajectory& t
     this->reset();
     CartesianTrajectory tmp(trajectory);
     swap(*this, tmp);
-    this->reference_frame_ = trajectory.get_reference_frame();
   }
   return *this;
 }

diff --git a/source/state_representation/src/trajectory/JointTrajectory.cpp b/source/state_representation/src/trajectory/JointTrajectory.cpp
@@ -109,7 +109,6 @@ JointTrajectory& JointTrajectory::operator=(const JointTrajectory& trajectory) {
     this->reset();
     JointTrajectory tmp(trajectory);
     swap(*this, tmp);
-    this->joint_names_ = trajectory.get_joint_names();
   }
   return *this;
 }