Joint trajectory feedback message (o3de#399)

* Filled in the position and velocity values in the feedback message, made it so that the feedback message is actually being sent out * Added a missing variable (jointInfo) --------- Signed-off-by: Antoni Puch <[email protected]>
Antoni-Robotec · Jul 24, 2023 · 41403d0 · 41403d0
1 parent 8f0973d
commit 41403d0
Showing 1 changed file with 35 additions and 39 deletions.
diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp
@@ -124,57 +124,52 @@ namespace ROS2
 
     void JointsTrajectoryComponent::UpdateFeedback()
     {
+        auto goalStatus = GetGoalStatus();
+        if (goalStatus != JointsTrajectoryRequests::TrajectoryActionStatus::Executing)
+        {
+            return;
+        }
+
         auto feedback = std::make_shared<control_msgs::action::FollowJointTrajectory::Feedback>();
-        trajectory_msgs::msg::JointTrajectoryPoint desiredPoint;
-        for (const auto& [jointName, hingeComponent] : m_manipulationJoints)
+
+        trajectory_msgs::msg::JointTrajectoryPoint desiredPoint = m_trajectoryGoal.trajectory.points.front();
+
+        trajectory_msgs::msg::JointTrajectoryPoint actualPoint;
+
+        size_t jointCount = m_trajectoryGoal.trajectory.joint_names.size();
+        for (size_t jointIndex = 0; jointIndex < jointCount; jointIndex++)
         {
+            AZStd::string jointName(m_trajectoryGoal.trajectory.joint_names[jointIndex].c_str());
             std::string jointNameStdString(jointName.c_str());
             feedback->joint_names.push_back(jointNameStdString);
 
-            AZ::Outcome<float, AZStd::string> result;
-            JointsManipulationRequestBus::EventResult(result, GetEntityId(), &JointsManipulationRequests::GetJointPosition, jointName);
-            auto currentJointPosition = result.GetValue();
-
-            desiredPoint.positions.push_back(static_cast<double>(currentJointPosition));
-            desiredPoint.velocities.push_back(0.0f); // Velocities not supported yet!
-            desiredPoint.accelerations.push_back(0.0f); // Accelerations not supported yet!
-            desiredPoint.effort.push_back(0.0f); // Effort not supported yet!
+            float currentJointPosition;
+            float currentJointVelocity;
+            auto &jointInfo = m_manipulationJoints[jointName];
+            PhysX::ArticulationJointRequestBus::Event(
+                jointInfo.m_entityComponentIdPair.GetEntityId(),
+                [&](PhysX::ArticulationJointRequests* articulationJointRequests)
+                {
+                    currentJointPosition = articulationJointRequests->GetJointPosition(jointInfo.m_axis);
+                    currentJointVelocity = articulationJointRequests->GetJointVelocity(jointInfo.m_axis);
+                });
+
+            actualPoint.positions.push_back(static_cast<double>(currentJointPosition));
+            actualPoint.velocities.push_back(static_cast<double>(currentJointVelocity));
+            // Acceleration should also be filled in somehow, or removed from the trajectory altogether.
         }
 
-        trajectory_msgs::msg::JointTrajectoryPoint actualPoint = desiredPoint;
         trajectory_msgs::msg::JointTrajectoryPoint currentError;
-
-        std::transform(
-            desiredPoint.positions.begin(),
-            desiredPoint.positions.end(),
-            actualPoint.positions.begin(),
-            currentError.positions.begin(),
-            std::minus<double>());
-
-        std::transform(
-            desiredPoint.velocities.begin(),
-            desiredPoint.velocities.end(),
-            actualPoint.velocities.begin(),
-            currentError.velocities.begin(),
-            std::minus<double>());
-
-        std::transform(
-            desiredPoint.accelerations.begin(),
-            desiredPoint.accelerations.end(),
-            actualPoint.accelerations.begin(),
-            currentError.accelerations.begin(),
-            std::minus<double>());
-
-        std::transform(
-            desiredPoint.effort.begin(),
-            desiredPoint.effort.end(),
-            actualPoint.effort.begin(),
-            currentError.effort.begin(),
-            std::minus<double>());
+        for (size_t jointIndex = 0; jointIndex < jointCount; jointIndex++)
+        {
+            currentError.positions.push_back(actualPoint.positions[jointIndex] - desiredPoint.positions[jointIndex]);
+            currentError.velocities.push_back(actualPoint.velocities[jointIndex] - desiredPoint.velocities[jointIndex]);
+        }
 
         feedback->desired = desiredPoint;
         feedback->actual = actualPoint;
         feedback->error = currentError;
+
         m_followTrajectoryServer->PublishFeedback(feedback);
     }
 
@@ -253,5 +248,6 @@ namespace ROS2
         }
         uint64_t deltaTimeNs = deltaTime * 1'000'000'000;
         FollowTrajectory(deltaTimeNs);
+        UpdateFeedback();
     }
 } // namespace ROS2