[jog_arm] Fix valid command flags (#2013)

* Rename the 'zero command flag' variables for readability
* Reset flags when incoming commands timeout
* Remove debug line, clang format

moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h

@@ -68,11 +68,11 @@ struct JogArmShared : public std::mutex
 
   double collision_velocity_scale = 1;
 
-  // Indicates that an incoming Cartesian command is all zero velocities
-  bool zero_cartesian_cmd_flag = true;
+  // Flag a valid incoming Cartesian command having nonzero velocities
+  bool have_nonzero_cartesian_cmd = false;
 
-  // Indicates that an incoming joint angle command is all zero velocities
-  bool zero_joint_cmd_flag = true;
+  // Flag a valid incoming joint angle command having nonzero velocities
+  bool have_nonzero_joint_cmd = false;
 
   // Indicates that we have not received a new command in some time
   bool command_is_stale = false;

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

@@ -161,12 +161,12 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables)
     {
       // Flag that incoming commands are all zero. May be used to skip calculations/publication
       shared_variables.lock();
-      bool zero_cartesian_cmd_flag = shared_variables.zero_cartesian_cmd_flag;
-      bool zero_joint_cmd_flag = shared_variables.zero_joint_cmd_flag;
+      bool have_nonzero_cartesian_cmd = shared_variables.have_nonzero_cartesian_cmd;
+      bool have_nonzero_joint_cmd = shared_variables.have_nonzero_joint_cmd;
       shared_variables.unlock();
 
       // Prioritize cartesian jogging above joint jogging
-      if (!zero_cartesian_cmd_flag)
+      if (have_nonzero_cartesian_cmd)
       {
         shared_variables.lock();
         cartesian_deltas = shared_variables.command_deltas;
@@ -175,7 +175,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables)
         if (!cartesianJogCalcs(cartesian_deltas, shared_variables))
           continue;
       }
-      else if (!zero_joint_cmd_flag)
+      else if (have_nonzero_joint_cmd)
       {
         shared_variables.lock();
         joint_deltas = shared_variables.joint_command_deltas;
@@ -194,14 +194,19 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables)
       bool stale_command = shared_variables.command_is_stale;
       shared_variables.unlock();
 
-      if (stale_command || (zero_cartesian_cmd_flag && zero_joint_cmd_flag))
+      if (stale_command || (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd))
       {
         suddenHalt(outgoing_command_);
-        zero_cartesian_cmd_flag = true;
-        zero_joint_cmd_flag = true;
+        have_nonzero_cartesian_cmd = false;
+        have_nonzero_joint_cmd = false;
+        // Reset the valid command flag so jogging stops until a new command arrives
+        shared_variables.lock();
+        shared_variables.have_nonzero_cartesian_cmd = false;
+        shared_variables.have_nonzero_joint_cmd = false;
+        shared_variables.unlock();
       }
 
-      bool valid_nonzero_command = !zero_cartesian_cmd_flag || !zero_joint_cmd_flag;
+      bool valid_nonzero_command = have_nonzero_cartesian_cmd || have_nonzero_joint_cmd;
 
       // Send the newest target joints
       shared_variables.lock();
@@ -228,7 +233,7 @@ void JogCalcs::startMainLoop(JogArmShared& shared_variables)
 
       // Store last zero-velocity message flag to prevent superfluous warnings.
       // Cartesian and joint commands must both be zero.
-      if (zero_cartesian_cmd_flag && zero_joint_cmd_flag)
+      if (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd)
       {
         // Avoid overflow
         if (zero_velocity_count < std::numeric_limits<int>::max())

moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp

@@ -169,14 +169,14 @@ void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamp
   }
 
   // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish
-  shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 &&
-                                              shared_variables_.command_deltas.twist.linear.y == 0.0 &&
-                                              shared_variables_.command_deltas.twist.linear.z == 0.0 &&
-                                              shared_variables_.command_deltas.twist.angular.x == 0.0 &&
-                                              shared_variables_.command_deltas.twist.angular.y == 0.0 &&
-                                              shared_variables_.command_deltas.twist.angular.z == 0.0;
-
-  if (!shared_variables_.zero_cartesian_cmd_flag)
+  shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.linear.y != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.linear.z != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.angular.x != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.angular.y != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.angular.z != 0.0;
+
+  if (shared_variables_.have_nonzero_cartesian_cmd)
   {
     shared_variables_.latest_nonzero_cmd_stamp = velocity_command.header.stamp;
   }
@@ -194,9 +194,9 @@ void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_co
   {
     all_zeros &= (delta == 0.0);
   };
-  shared_variables_.zero_joint_cmd_flag = all_zeros;
+  shared_variables_.have_nonzero_joint_cmd = !all_zeros;
 
-  if (!shared_variables_.zero_joint_cmd_flag)
+  if (shared_variables_.have_nonzero_joint_cmd)
   {
     shared_variables_.latest_nonzero_cmd_stamp = joint_command.header.stamp;
   }

moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp

@@ -182,14 +182,14 @@ void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConst
   }
 
   // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish
-  shared_variables_.zero_cartesian_cmd_flag = shared_variables_.command_deltas.twist.linear.x == 0.0 &&
-                                              shared_variables_.command_deltas.twist.linear.y == 0.0 &&
-                                              shared_variables_.command_deltas.twist.linear.z == 0.0 &&
-                                              shared_variables_.command_deltas.twist.angular.x == 0.0 &&
-                                              shared_variables_.command_deltas.twist.angular.y == 0.0 &&
-                                              shared_variables_.command_deltas.twist.angular.z == 0.0;
-
-  if (!shared_variables_.zero_cartesian_cmd_flag)
+  shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.linear.y != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.linear.z != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.angular.x != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.angular.y != 0.0 ||
+                                                 shared_variables_.command_deltas.twist.angular.z != 0.0;
+
+  if (shared_variables_.have_nonzero_cartesian_cmd)
   {
     shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp;
   }
@@ -208,9 +208,9 @@ void JogROSInterface::deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg)
   {
     all_zeros &= (delta == 0.0);
   };
-  shared_variables_.zero_joint_cmd_flag = all_zeros;
+  shared_variables_.have_nonzero_joint_cmd = !all_zeros;
 
-  if (!shared_variables_.zero_joint_cmd_flag)
+  if (shared_variables_.have_nonzero_joint_cmd)
   {
     shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp;
   }