Add option to wait convergence of joint trajectory angle and velocity (…

…#122)
openspur · Mar 28, 2024 · 97d8a8b · 97d8a8b
1 parent 7d429e0
commit 97d8a8b
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 1 deletion.
diff --git a/src/ypspur_ros.cpp b/src/ypspur_ros.cpp
@@ -97,6 +97,7 @@ class YpspurRosNode
   std::map<std::string, double> params_;
   int key_;
   bool simulate_;
+  bool wait_convergence_of_joint_trajectory_angle_vel_;
 
   double tf_time_offset_;
 
@@ -451,6 +452,8 @@ class YpspurRosNode
       ROS_WARN("simulate_control parameter is deprecated. Use simulate parameter instead");
       simulate_ = true;
     }
+    pnh_.param(
+        "wait_convergence_of_joint_trajectory_angle_vel", wait_convergence_of_joint_trajectory_angle_vel_, true);
     pnh_.param("ypspur_bin", ypspur_bin_, std::string("ypspur-coordinator"));
     pnh_.param("param_file", param_file_, std::string(""));
     pnh_.param("tf_time_offset", tf_time_offset_, 0.0);
@@ -1099,7 +1102,8 @@ class YpspurRosNode
 
           if (done)
           {
-            if ((joints_[jid].vel_end_ > 0.0 &&
+            if (!wait_convergence_of_joint_trajectory_angle_vel_ ||
+                (joints_[jid].vel_end_ > 0.0 &&
                  joints_[jid].angle_ref_ > joint.position[jid] &&
                  joints_[jid].angle_ref_ < joint.position[jid] + joints_[jid].vel_ref_ * dt) ||
                 (joints_[jid].vel_end_ < 0.0 &&
@@ -1108,6 +1112,7 @@ class YpspurRosNode
             {
               joints_[jid].control_ = JointParams::VELOCITY;
               joints_[jid].vel_ref_ = joints_[jid].vel_end_;
+              YP::YP_joint_vel(joints_[jid].id_, joints_[jid].vel_ref_);
             }
           }
         }

diff --git a/test/src/joint_trajectory.cpp b/test/src/joint_trajectory.cpp
@@ -117,6 +117,32 @@ TEST(JointTrajectory, CommandValidation)
   }
   ASSERT_NEAR(joint_states->velocity[0], 0.0, 0.1)
       << "Outdated joint_trajectory must be ignored";
+
+  // Publish overshoot command
+  cmd.header.stamp = ros::Time::now();
+  cmd.joint_names.resize(1);
+  cmd.joint_names[0] = "joint0";
+  cmd.points.resize(1);
+  cmd.points[0].time_from_start = ros::Duration(1);
+  cmd.points[0].positions.resize(1);
+  cmd.points[0].positions[0] = 1.0;
+  cmd.points[0].velocities.resize(1);
+  cmd.points[0].velocities[0] = 4.0;
+  pub_cmd.publish(cmd);
+  wait.sleep();
+
+  for (int i = 0; i < 50; ++i)
+  {
+    wait.sleep();
+    ros::spinOnce();
+  }
+
+  ASSERT_TRUE(static_cast<bool>(joint_states));
+  ASSERT_EQ(joint_states->name.size(), 1u);
+  ASSERT_EQ(joint_states->name[0], "joint0");
+  ASSERT_EQ(joint_states->velocity.size(), 1u);
+  ASSERT_NEAR(joint_states->velocity[0], 4.0, 0.1)
+      << "Joint velocity should be near the target value";
 }
 
 int main(int argc, char** argv)

diff --git a/test/test/joint_trajectory.test b/test/test/joint_trajectory.test
@@ -8,5 +8,6 @@
     <param name="param_file" value="$(find ypspur_ros)/test/config/test.param" />
     <param name="joint0_enable" value="true" />
     <param name="hz" value="20.0" />
+    <param name="wait_convergence_of_joint_trajectory_angle_vel" value="false" />
   </node>
 </launch>