Merge branch 'main' into computeCartesianPath

.github/workflows/ci.yaml

@@ -31,8 +31,10 @@ jobs:
           - IMAGE: jazzy-ci
             ROS_DISTRO: jazzy
     env:
+      # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option
+      # https://stackoverflow.com/a/41673702
       CXXFLAGS: >-
-        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
+        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option
       CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
       DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
       UPSTREAM_WORKSPACE: >

moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp

@@ -95,7 +95,7 @@ int main(int argc, char* argv[])
 
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
-  KinematicState current_state = servo.getCurrentRobotState();
+  KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */);
   updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -119,7 +119,7 @@ int main(int argc, char* argv[])
 
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
-  KinematicState current_state = servo.getCurrentRobotState();
+  KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */);
   updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   bool satisfies_linear_tolerance = false;

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -98,7 +98,7 @@ int main(int argc, char* argv[])
 
   // create command queue to build trajectory message and add current robot state
   std::deque<KinematicState> joint_cmd_rolling_window;
-  KinematicState current_state = servo.getCurrentRobotState();
+  KinematicState current_state = servo.getCurrentRobotState(true /* wait for updated state */);
   updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
 
   RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -118,9 +118,10 @@ class Servo
 
   /**
    * \brief Get the current state of the robot as given by planning scene monitor.
+   * @param block_for_current_state If true, we explicitly wait for a new robot state
    * @return The current state of the robot.
    */
-  KinematicState getCurrentRobotState() const;
+  KinematicState getCurrentRobotState(bool block_for_current_state) const;
 
   /**
    * \brief Smoothly halt at a commanded state when command goes stale.

moveit_ros/moveit_servo/src/servo.cpp

@@ -158,7 +158,6 @@ void Servo::setSmoothingPlugin()
     RCLCPP_ERROR(logger_, "Smoothing plugin could not be initialized");
     std::exit(EXIT_FAILURE);
   }
-  resetSmoothing(getCurrentRobotState());
 }
 
 void Servo::doSmoothing(KinematicState& state)
@@ -526,9 +525,6 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     // Adjust joint position based on scaled down velocity
     target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period);
 
-    // Apply smoothing to the positions if a smoother was provided.
-    doSmoothing(target_state);
-
     // Apply collision scaling to the joint position delta
     target_state.positions =
         current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions);
@@ -548,8 +544,8 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     }
   }
 
-  // Update internal state of filter with final calculated command.
-  resetSmoothing(target_state);
+  // Apply smoothing to the positions if a smoother was provided.
+  doSmoothing(target_state);
 
   return target_state;
 }
@@ -647,8 +643,21 @@ std::optional<PoseCommand> Servo::toPlanningFrame(const PoseCommand& command, co
   return PoseCommand{ planning_frame, planning_to_command_tf * command.pose };
 }
 
-KinematicState Servo::getCurrentRobotState() const
+KinematicState Servo::getCurrentRobotState(bool block_for_current_state) const
 {
+  if (block_for_current_state)
+  {
+    bool have_current_state = false;
+    while (rclcpp::ok() && !have_current_state)
+    {
+      have_current_state = planning_scene_monitor_->getStateMonitor()->waitForCurrentState(
+          rclcpp::Clock(RCL_ROS_TIME).now(), ROBOT_STATE_WAIT_TIME /* s */);
+      if (!have_current_state)
+      {
+        RCLCPP_WARN(logger_, "Waiting for the current state");
+      }
+    }
+  }
   moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
   return extractRobotState(robot_state, servo_params_.move_group_name);
 }
@@ -665,9 +674,6 @@ std::pair<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_sta
     // set target velocity
     target_state.velocities *= 0.0;
 
-    // apply smoothing: this will change target position/velocity to make slow down gradual
-    doSmoothing(target_state);
-
     // scale velocity in case of obstacle
     target_state.velocities *= collision_velocity_scale_;
 
@@ -681,7 +687,9 @@ std::pair<bool, KinematicState> Servo::smoothHalt(const KinematicState& halt_sta
     }
   }
 
-  resetSmoothing(target_state);
+  // apply smoothing: this will change target position/velocity to make slow down gradual
+  doSmoothing(target_state);
+
   return std::make_pair(stopped, target_state);
 }
 

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -152,12 +152,11 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request
   else
   {
     // Reset the smoothing plugin with the robot's current state in case the robot moved between pausing and unpausing.
-    last_commanded_state_ = servo_->getCurrentRobotState();
+    last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */);
     servo_->resetSmoothing(last_commanded_state_);
 
     // clear out the command rolling window and reset last commanded state to be the current state
     joint_cmd_rolling_window_.clear();
-    last_commanded_state_ = servo_->getCurrentRobotState();
 
     // reactivate collision checking
     servo_->setCollisionChecking(true);
@@ -301,8 +300,10 @@ void ServoNode::servoLoop()
     RCLCPP_INFO(node_->get_logger(), "Waiting to receive robot state update.");
     rclcpp::sleep_for(std::chrono::seconds(1));
   }
-  KinematicState current_state = servo_->getCurrentRobotState();
+  KinematicState current_state = servo_->getCurrentRobotState(true /* block for current robot state */);
   last_commanded_state_ = current_state;
+  // Ensure the filter is up to date
+  servo_->resetSmoothing(current_state);
 
   // Get the robot state and joint model group info.
   moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
@@ -314,6 +315,7 @@ void ServoNode::servoLoop()
     // Skip processing if servoing is disabled.
     if (servo_paused_)
     {
+      servo_->resetSmoothing(current_state);
       servo_frequency.sleep();
       continue;
     }
@@ -329,7 +331,7 @@ void ServoNode::servoLoop()
     {
       // if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state
       joint_cmd_rolling_window_.clear();
-      current_state = servo_->getCurrentRobotState();
+      current_state = servo_->getCurrentRobotState(false /* block for current robot state */);
       current_state.velocities *= 0.0;
     }
 

moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp

@@ -438,7 +438,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     debug_msg.step = w * sizeof(float);
     debug_msg.data.resize(img_size * sizeof(float));
-    mesh_filter_->getModelDepth(reinterpret_cast<double*>(&debug_msg.data[0]));
+    mesh_filter_->getModelDepth(reinterpret_cast<float*>(&debug_msg.data[0]));
     pub_model_depth_image_.publish(debug_msg, *info_msg);
 
     sensor_msgs::msg::Image filtered_depth_msg;
@@ -449,7 +449,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     filtered_depth_msg.step = w * sizeof(float);
     filtered_depth_msg.data.resize(img_size * sizeof(float));
-    mesh_filter_->getFilteredDepth(reinterpret_cast<double*>(&filtered_depth_msg.data[0]));
+    mesh_filter_->getFilteredDepth(reinterpret_cast<float*>(&filtered_depth_msg.data[0]));
     pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg);
 
     sensor_msgs::msg::Image label_msg;
@@ -481,7 +481,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     if (filtered_data.size() < img_size)
       filtered_data.resize(img_size);
 
-    mesh_filter_->getFilteredDepth(reinterpret_cast<double*>(&filtered_data[0]));
+    mesh_filter_->getFilteredDepth(reinterpret_cast<float*>(&filtered_data[0]));
     unsigned short* msg_data = reinterpret_cast<unsigned short*>(&filtered_msg.data[0]);
     for (std::size_t i = 0; i < img_size; ++i)
     {

moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h

@@ -106,7 +106,7 @@ class GLRenderer
    * \author Suat Gedikli ([email protected])
    * \param[out] buffer pointer to memory where the depth values need to be stored
    */
-  void getDepthBuffer(double* buffer) const;
+  void getDepthBuffer(float* buffer) const;
 
   /**
    * \brief loads, compiles, links and adds GLSL shaders from files to the current OpenGL context.

moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h

@@ -131,7 +131,7 @@ class MeshFilterBase
    * \author Suat Gedikli ([email protected])
    * \param[out] depth pointer to buffer to be filled with depth values.
    */
-  void getFilteredDepth(double* depth) const;
+  void getFilteredDepth(float* depth) const;
 
   /**
    * \brief retrieves the labels of the rendered model
@@ -149,7 +149,7 @@ class MeshFilterBase
    * \author Suat Gedikli ([email protected])
    * \param[out] depth pointer to buffer to be filled with depth values.
    */
-  void getModelDepth(double* depth) const;
+  void getModelDepth(float* depth) const;
 
   /**
    * \brief set the shadow threshold. points that are further away than the rendered model are filtered out.

moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h

@@ -103,13 +103,13 @@ class SensorModel
      * \brief transforms depth values from rendered model to metric depth values
      * \param[in,out] depth pointer to floating point depth buffer
      */
-    virtual void transformModelDepthToMetricDepth(double* depth) const;
+    virtual void transformModelDepthToMetricDepth(float* depth) const;
 
     /**
      * \brief transforms depth values from filtered depth to metric depth values
      * \param[in,out] depth pointer to floating point depth buffer
      */
-    virtual void transformFilteredDepthToMetricDepth(double* depth) const;
+    virtual void transformFilteredDepthToMetricDepth(float* depth) const;
 
     /**
      * \brief sets  the image size

moveit_ros/perception/mesh_filter/src/gl_renderer.cpp

@@ -213,7 +213,7 @@ void mesh_filter::GLRenderer::getColorBuffer(unsigned char* buffer) const
   glBindFramebuffer(GL_FRAMEBUFFER, 0);
 }
 
-void mesh_filter::GLRenderer::getDepthBuffer(double* buffer) const
+void mesh_filter::GLRenderer::getDepthBuffer(float* buffer) const
 {
   glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_);
   glBindTexture(GL_TEXTURE_2D, depth_id_);

moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp

@@ -227,7 +227,7 @@ void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const
   job->wait();
 }
 
-void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const
+void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const
 {
   JobPtr job1 =
       std::make_shared<FilterJob<void>>([&renderer = *mesh_renderer_, depth] { renderer.getDepthBuffer(depth); });
@@ -243,7 +243,7 @@ void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const
   job2->wait();
 }
 
-void mesh_filter::MeshFilterBase::getFilteredDepth(double* depth) const
+void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const
 {
   JobPtr job1 = std::make_shared<FilterJob<void>>([&filter = *depth_filter_, depth] { filter.getDepthBuffer(depth); });
   JobPtr job2 = std::make_shared<FilterJob<void>>(

moveit_ros/perception/mesh_filter/src/sensor_model.cpp

@@ -102,7 +102,7 @@ inline bool isAligned16(const void* pointer)
 #endif
 }  // namespace
 
-void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(double* depth) const
+void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(float* depth) const
 {
 #if HAVE_SSE_EXTENSIONS
   const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_);
@@ -161,7 +161,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(doub
   const float nf = near * far;
   const float f_n = far - near;
 
-  const double* depth_end = depth + width_ * height_;
+  const float* depth_end = depth + width_ * height_;
   while (depth < depth_end)
   {
     if (*depth != 0 && *depth != 1)
@@ -178,7 +178,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(doub
 #endif
 }
 
-void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(double* depth) const
+void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(float* depth) const
 {
 #if HAVE_SSE_EXTENSIONS
   //* SSE version
@@ -223,7 +223,7 @@ void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(d
     ++mmDepth;
   }
 #else
-  const double* depth_end = depth + width_ * height_;
+  const float* depth_end = depth + width_ * height_;
   const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_;
   const float offset = near_clipping_plane_distance_;
   while (depth < depth_end)