[jog_arm] Changes before porting to ROS2 (moveit#2151)

* throttle warning logs

* ROS1 Basic improvements and changes

* Fixes to drift dimensions, singularity velocity scaling

* tf name changes, const fixes, slight logic changes

* Reverting enforceSRDFAccelVelLimits changes for now

* Move ROS_LOG_THROTTLE_PERIOD to cpp files

* Clang formatting

* Track staleness of joint and twist seperately

* Ensure joint_trajectory output is always populated with something, even when no jog

* Fix joint trajectory redundant points for gazebo pub

* Fix crazy joint jog from bad Eigen init

* Fix variable type in addJointIncrements()

* Initialize last sent command in constructor

* More explicit joint_jog_cmd_ and twist_stamped_cmd_ names

* Add comment clarying transform calculation / use

Co-authored-by: Tyler Weaver <tyler@picknik.ai>
Co-authored-by: AndyZe <andyz@utexas.edu>

moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_parameters.h

@@ -43,9 +43,6 @@ namespace moveit_jog_arm
 // Size of queues used in ros pub/sub/service
 constexpr size_t ROS_QUEUE_SIZE = 2;
 
-// Seconds to throttle logs inside loops
-constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30;
-
 // ROS params to be read. See the yaml file in /config for a description of each.
 struct JogArmParameters
 {

moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h

@@ -114,7 +114,6 @@ class JogCalcs
    * Is handled differently for position vs. velocity control.
    */
   void suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory);
-  void suddenHalt(Eigen::ArrayXd& delta_theta);
 
   /** \brief  Scale the delta theta to match joint velocity/acceleration limits */
   void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta);
@@ -127,7 +126,7 @@ class JogCalcs
    */
   double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity,
                                              const Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
-                                             const Eigen::MatrixXd& jacobian, const Eigen::MatrixXd& pseudo_inverse);
+                                             const Eigen::MatrixXd& pseudo_inverse);
 
   /**
    * Slow motion down if close to singularity or collision.
@@ -137,12 +136,15 @@ class JogCalcs
   void applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale);
 
   /** \brief Compose the outgoing JointTrajectory message */
-  void composeJointTrajMessage(sensor_msgs::JointState& joint_state,
+  void composeJointTrajMessage(const sensor_msgs::JointState& joint_state,
                                trajectory_msgs::JointTrajectory& joint_trajectory) const;
 
   /** \brief Smooth position commands with a lowpass filter */
   void lowPassFilterPositions(sensor_msgs::JointState& joint_state);
 
+  /** \brief Set the filters to the specified values */
+  void resetLowPassFilters(const sensor_msgs::JointState& joint_state);
+
   /** \brief Convert an incremental position command to joint velocity message */
   void calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta);
 
@@ -207,14 +209,17 @@ class JogCalcs
   // Flag for staying inactive while there are no incoming commands
   bool wait_for_jog_commands_ = true;
 
+  // Flag saying if the filters were updated during the timer callback
+  bool updated_filters_ = false;
+
   // Nonzero status flags
   bool have_nonzero_twist_stamped_ = false;
   bool have_nonzero_joint_jog_ = false;
   bool have_nonzero_command_ = false;
 
   // Incoming command messages
-  geometry_msgs::TwistStamped twist_stamped_;
-  control_msgs::JointJog joint_jog_;
+  geometry_msgs::TwistStamped twist_stamped_cmd_;
+  control_msgs::JointJog joint_jog_cmd_;
 
   const moveit::core::JointModelGroup* joint_model_group_;
 
@@ -229,6 +234,8 @@ class JogCalcs
 
   std::vector<LowPassFilter> position_filters_;
 
+  trajectory_msgs::JointTrajectoryConstPtr last_sent_command_;
+
   // ROS
   ros::Timer timer_;
   ros::Duration period_;
@@ -246,7 +253,8 @@ class JogCalcs
   StatusCode status_ = StatusCode::NO_WARNING;
   bool stop_requested_ = false;
   bool paused_ = false;
-  bool command_is_stale_ = false;
+  bool twist_command_is_stale_ = false;
+  bool joint_command_is_stale_ = false;
   bool ok_to_publish_ = false;
   double collision_velocity_scale_ = 1.0;
 
@@ -269,9 +277,12 @@ class JogCalcs
 
   // latest_state_mutex_ is used to protect the state below it
   mutable std::mutex latest_state_mutex_;
-  Eigen::Isometry3d tf_moveit_to_cmd_frame_;
+  Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_;
   geometry_msgs::TwistStampedConstPtr latest_twist_stamped_;
   control_msgs::JointJogConstPtr latest_joint_jog_;
-  ros::Time latest_command_stamp_ = ros::Time(0.);
+  ros::Time latest_twist_command_stamp_ = ros::Time(0.);
+  ros::Time latest_joint_command_stamp_ = ros::Time(0.);
+  bool latest_nonzero_twist_stamped_ = false;
+  bool latest_nonzero_joint_jog_ = false;
 };
 }  // namespace moveit_jog_arm

moveit_experimental/moveit_jog_arm/src/collision_check.cpp

@@ -44,7 +44,8 @@
 
 static const char LOGNAME[] = "collision_check";
 static const double MIN_RECOMMENDED_COLLISION_RATE = 10;
-constexpr double EPSILON = 1e-6;  // For very small numeric comparisons
+constexpr double EPSILON = 1e-6;                // For very small numeric comparisons
+constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30;  // Seconds to throttle logs inside loops
 
 namespace moveit_jog_arm
 {