add current state to command queue if it is empty. This is needed sin…

…ce the time stamp is set in the updateSlidingWindow function now.

Signed-off-by: Paul Gesel <paulgesel@gmail.com>

moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp

@@ -93,8 +93,10 @@ int main(int argc, char* argv[])
   std::chrono::seconds time_elapsed(0);
   auto start_time = std::chrono::steady_clock::now();
 
-  // create command queue to build trajectory message
+  // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
+  KinematicState current_state = servo.getCurrentRobotState();
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
   while (rclcpp::ok())

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -115,12 +115,12 @@ int main(int argc, char* argv[])
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
 
-  KinematicState joint_state;
-  KinematicState current_state = servo.getCurrentRobotState();
   rclcpp::WallRate servo_rate(1 / servo_params.publish_period);
 
-  // create command queue to build trajectory message
+  // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
+  KinematicState current_state = servo.getCurrentRobotState();
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
 
   bool satisfies_linear_tolerance = false;
   bool satisfies_angular_tolerance = false;
@@ -146,7 +146,7 @@ int main(int argc, char* argv[])
     }
 
     // get next servo command
-    joint_state = servo.getNextJointState(robot_state, target_pose);
+    KinematicState joint_state = servo.getNextJointState(robot_state, target_pose);
     StatusCode status = servo.getStatus();
     if (status != StatusCode::INVALID)
     {

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -96,8 +96,10 @@ int main(int argc, char* argv[])
   std::chrono::seconds time_elapsed(0);
   auto start_time = std::chrono::steady_clock::now();
 
-  // create command queue to build trajectory message
+  // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
+  KinematicState current_state = servo.getCurrentRobotState();
+  updateSlidingWindow(current_state, joint_cmd_rolling_window, 0.0, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
   while (rclcpp::ok())

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -332,7 +332,9 @@ void ServoNode::servoLoop()
     else
     {
       current_state = servo_->getCurrentRobotState();
+      current_state.velocities *= 0.0;
       joint_cmd_rolling_window_.clear();
+      updateSlidingWindow(current_state, joint_cmd_rolling_window_, 0.0, cur_time);
     }
 
     // Get the robot state and joint model group info.