Merge pull request #104 from marrts/update_stable_0-22

.github/workflows/code_quality.yml

@@ -26,7 +26,7 @@ jobs:
               TARGET_CMAKE_ARGS: "-DCMAKE_CXX_COMPILER_LAUNCHER=ccache -DCMAKE_BUILD_TYPE=Debug -DTESSERACT_ENABLE_CLANG_TIDY=ON -DTESSERACT_ENABLE_TESTING=ON"
 
     container:
-      image: ghcr.io/tesseract-robotics/tesseract_qt:${{ matrix.os }}-0.21
+      image: ghcr.io/tesseract-robotics/tesseract_qt:${{ matrix.os }}-0.22
       env:
         CCACHE_DIR: "${{ github.workspace }}/${{ matrix.os }}/.ccache"
         DEBIAN_FRONTEND: noninteractive
@@ -47,7 +47,7 @@ jobs:
           apt install -y clang-tidy ros-${{ matrix.distro }}-ros-base
 
       - name: Build and Tests
-        uses: tesseract-robotics/colcon-action@v5
+        uses: tesseract-robotics/colcon-action@v8
         with:
           before-script: apt install ros-${{ matrix.distro }}-ros-base && source /opt/tesseract_qt/install/setup.bash && source /opt/ros/${{ matrix.distro }}/setup.bash
           ccache-prefix: ${{ matrix.distro }}

.github/workflows/ubuntu.yml

@@ -33,7 +33,7 @@ jobs:
             distro: rolling
             vcs-file: .github/workflows/dependencies.repos
     container:
-      image: ghcr.io/tesseract-robotics/tesseract_qt:${{ matrix.os }}-0.21
+      image: ghcr.io/tesseract-robotics/tesseract_qt:${{ matrix.os }}-0.22
       env:
         CCACHE_DIR: ${{ github.workspace }}/${{ matrix.distro }}/.ccache
         DEBIAN_FRONTEND: noninteractive
@@ -53,7 +53,7 @@ jobs:
           apt install -y ros-${{ matrix.distro }}-ros-base
 
       - name: Build and Tests
-        uses: tesseract-robotics/colcon-action@v5
+        uses: tesseract-robotics/colcon-action@v8
         with:
           before-script: source /opt/tesseract_qt/install/setup.bash && source /opt/ros/${{ matrix.distro }}/setup.bash
           ccache-prefix: ${{ matrix.distro }}

.github/workflows/unstable.yml

@@ -53,7 +53,7 @@ jobs:
           apt install -y ros-${{ matrix.distro }}-ros-base
 
       - name: Build and Tests
-        uses: tesseract-robotics/colcon-action@v5
+        uses: tesseract-robotics/colcon-action@v8
         with:
           before-script: source /opt/tesseract_qt/install/setup.bash && source /opt/ros/${{ matrix.distro }}/setup.bash
           ccache-prefix: ${{ matrix.distro }}

docker/docker-compose.yml

@@ -5,15 +5,15 @@ services:
       context: ..
       dockerfile: docker/Dockerfile
       args:
-        - TAG=focal-0.20
+        - TAG=focal-0.22
         - ROS_DISTRO=foxy
     environment:
       DISPLAY: $DISPLAY
       XAUTHORITY: $XAUTHORITY
       NVIDIA_DRIVER_CAPABILITIES: all
       ROS_LOG_DIR: /tmp
     container_name: tesseract_ros2
-    image: ghcr.io/tesseract-robotics/tesseract_ros2:foxy-0.20
+    image: ghcr.io/tesseract-robotics/tesseract_ros2:foxy-0.22
     stdin_open: true
     tty: true
     network_mode: host

tesseract_monitoring/include/tesseract_monitoring/contact_monitor.h

@@ -105,7 +105,7 @@ class ContactMonitor
   callbackComputeContactResultVector(tesseract_msgs::srv::ComputeContactResultVector::Request::SharedPtr request,
                                      tesseract_msgs::srv::ComputeContactResultVector::Response::SharedPtr response);
 
-  void callbackTesseractEnvDiff(const tesseract_msgs::msg::EnvironmentState::SharedPtr state);
+  void callbackTesseractEnvDiff(const tesseract_msgs::msg::EnvironmentState::SharedPtr state);  // NOLINT
 
 private:
   std::string monitor_namespace_;

tesseract_monitoring/include/tesseract_monitoring/current_state_monitor.h

@@ -154,7 +154,7 @@ class CurrentStateMonitor
   /** @brief Wait for at most \e wait_time seconds (default 1s) for a robot state more recent than t
    *  @return true on success, false if up-to-date robot state wasn't received within \e wait_time
    */
-  bool waitForCurrentState(rclcpp::Time t /* = node_->now() */, double wait_time = 1.0) const;
+  bool waitForCurrentState(const rclcpp::Time& t, double wait_time = 1.0) const;
 
   /** @brief Wait for at most \e wait_time seconds until the complete robot state is known.
       @return true if the full state is known */
@@ -188,7 +188,7 @@ class CurrentStateMonitor
   void enableCopyDynamics(bool enabled) { copy_dynamics_ = enabled; }
 
 private:
-  void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr joint_state);
+  void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr joint_state);  // NOLINT
   bool isPassiveOrMimicDOF(const std::string& dof) const;
 
   rclcpp::Node::SharedPtr node_;

tesseract_monitoring/include/tesseract_monitoring/environment_monitor.h

@@ -74,14 +74,16 @@ class ROSEnvironmentMonitor : public tesseract_environment::EnvironmentMonitor
    * @param robot_description The name of the ROS parameter that contains the URDF (in string format)
    * @param monitor_namespace A name identifying this monitor, must be unique
    */
-  ROSEnvironmentMonitor(rclcpp::Node::SharedPtr node, std::string robot_description, std::string monitor_namespace);
+  ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node,
+                        std::string robot_description,
+                        std::string monitor_namespace);  // NOLINT
 
   /**
    * @brief Constructor
    * @param env The environment
    * @param monitor_namespace A name identifying this monitor, must be unique
    */
-  ROSEnvironmentMonitor(rclcpp::Node::SharedPtr node,
+  ROSEnvironmentMonitor(const rclcpp::Node::SharedPtr& node,  // NOLINT
                         std::shared_ptr<tesseract_environment::Environment> env,
                         std::string monitor_namespace);
 
@@ -137,9 +139,9 @@ class ROSEnvironmentMonitor : public tesseract_environment::EnvironmentMonitor
    */
   bool initialize();
 
-  rclcpp::Time last_update_time_ = rclcpp::Time(0l, RCL_ROS_TIME);        /// Last time the state was updated
-  rclcpp::Time last_robot_motion_time_ = rclcpp::Time(0l, RCL_ROS_TIME);  /// Last time the robot has moved
-  bool enforce_next_state_update_;  /// flag to enforce immediate state update in onStateUpdate()
+  rclcpp::Time last_update_time_ = rclcpp::Time(0L, RCL_ROS_TIME);        /// Last time the state was updated
+  rclcpp::Time last_robot_motion_time_ = rclcpp::Time(0L, RCL_ROS_TIME);  /// Last time the robot has moved
+  bool enforce_next_state_update_{ false };  /// flag to enforce immediate state update in onStateUpdate()
 
   rclcpp::Node::SharedPtr node_;
   rclcpp::Node::SharedPtr internal_node_;
@@ -150,7 +152,7 @@ class ROSEnvironmentMonitor : public tesseract_environment::EnvironmentMonitor
   // variables for planning scene publishing
   rclcpp::Publisher<tesseract_msgs::msg::EnvironmentState>::SharedPtr environment_publisher_;
   std::unique_ptr<std::thread> publish_environment_;
-  double publish_environment_frequency_;
+  double publish_environment_frequency_{ 30.0 };
 
   // variables for monitored environment
   rclcpp::Subscription<tesseract_msgs::msg::EnvironmentState>::SharedPtr monitored_environment_subscriber_;
@@ -190,13 +192,13 @@ class ROSEnvironmentMonitor : public tesseract_environment::EnvironmentMonitor
   void environmentPublishingThread();
 
   // called by current_state_monitor_ when robot state (as monitored on joint state topic) changes
-  void onJointStateUpdate(const sensor_msgs::msg::JointState::ConstSharedPtr joint_state);
+  void onJointStateUpdate(const sensor_msgs::msg::JointState::ConstSharedPtr joint_state);  // NOLINT
 
   // called by state_update_timer_ when a state update is pending
   void updateJointStateTimerCallback();
 
   // Callback for a new state msg
-  void newEnvironmentStateCallback(const tesseract_msgs::msg::EnvironmentState::ConstSharedPtr env);
+  void newEnvironmentStateCallback(const tesseract_msgs::msg::EnvironmentState::ConstSharedPtr env);  // NOLINT
 
   /** @brief Callback for modifying the environment via service request */
   void modifyEnvironmentCallback(tesseract_msgs::srv::ModifyEnvironment::Request::SharedPtr req,
@@ -224,7 +226,7 @@ class ROSEnvironmentMonitor : public tesseract_environment::EnvironmentMonitor
 
   /// True when we need to update the RobotState from current_state_monitor_
   // This field is protected by state_pending_mutex_
-  volatile bool state_update_pending_;
+  volatile bool state_update_pending_{ false };
 
   /// the amount of time to wait in between updates to the robot state
   // This field is protected by state_pending_mutex_
@@ -237,7 +239,7 @@ class ROSEnvironmentMonitor : public tesseract_environment::EnvironmentMonitor
 
   /// Last time the state was updated from current_state_monitor_
   // Only access this from callback functions (and constructor)
-  rclcpp::Time last_robot_state_update_wall_time_ = rclcpp::Time(0l, RCL_SYSTEM_TIME);
+  rclcpp::Time last_robot_state_update_wall_time_ = rclcpp::Time(0L, RCL_SYSTEM_TIME);
 
   std::atomic<bool> publish_{ false };
 

tesseract_monitoring/include/tesseract_monitoring/environment_monitor_interface.h

@@ -45,8 +45,8 @@ class ROSEnvironmentMonitorInterface : public tesseract_environment::Environment
   using UPtr = std::unique_ptr<ROSEnvironmentMonitorInterface>;
   using ConstUPtr = std::unique_ptr<const ROSEnvironmentMonitorInterface>;
 
-  ROSEnvironmentMonitorInterface(rclcpp::Node::SharedPtr node, const std::string env_name);
-  virtual ~ROSEnvironmentMonitorInterface() override = default;
+  ROSEnvironmentMonitorInterface(rclcpp::Node::SharedPtr node, std::string env_name);
+  ~ROSEnvironmentMonitorInterface() override = default;
   ROSEnvironmentMonitorInterface(const ROSEnvironmentMonitorInterface&) = default;
   ROSEnvironmentMonitorInterface& operator=(const ROSEnvironmentMonitorInterface&) = default;
   ROSEnvironmentMonitorInterface(ROSEnvironmentMonitorInterface&&) = default;

tesseract_monitoring/src/contact_monitor.cpp

@@ -41,7 +41,7 @@ namespace tesseract_monitoring
 {
 ContactMonitor::ContactMonitor(std::string monitor_namespace,
                                tesseract_environment::Environment::UPtr env,
-                               rclcpp::Node::SharedPtr node,
+                               rclcpp::Node::SharedPtr node,  // NOLINT
                                std::vector<std::string> monitored_link_names,
                                std::vector<std::string> disabled_link_names,
                                tesseract_collision::ContactTestType type,
@@ -74,16 +74,19 @@ ContactMonitor::ContactMonitor(std::string monitor_namespace,
   std::cout << ((disabled_link_names_.empty()) ? "Empty" : "Not Empty") << std::endl;
 
   joint_states_sub_ = internal_node_->create_subscription<sensor_msgs::msg::JointState>(
-      joint_state_topic, 1000, std::bind(&ContactMonitor::callbackJointState, this, std::placeholders::_1));
+      joint_state_topic, 1000, std::bind(&ContactMonitor::callbackJointState, this, std::placeholders::_1));  // NOLINT
   std::string contact_results_topic = R"(/)" + monitor_namespace_ + DEFAULT_PUBLISH_CONTACT_RESULTS_TOPIC;
   std::string compute_contact_results = R"(/)" + monitor_namespace_ + DEFAULT_COMPUTE_CONTACT_RESULTS_SERVICE;
 
   contact_results_pub_ = internal_node_->create_publisher<tesseract_msgs::msg::ContactResultVector>(
       contact_results_topic, rclcpp::QoS(100));
   compute_contact_results_ = internal_node_->create_service<tesseract_msgs::srv::ComputeContactResultVector>(
       compute_contact_results,
-      std::bind(
-          &ContactMonitor::callbackComputeContactResultVector, this, std::placeholders::_1, std::placeholders::_2),
+      std::bind(  // NOLINT
+          &ContactMonitor::callbackComputeContactResultVector,
+          this,
+          std::placeholders::_1,
+          std::placeholders::_2),
       rmw_qos_profile_services_default);
 }
 
@@ -161,8 +164,9 @@ void ContactMonitor::computeCollisionReportThread()
       contacts_vector.clear();
       contacts_msg.contacts.clear();
 
-      monitor_->environment().setState(msg->name,
-                                       Eigen::Map<Eigen::VectorXd>(msg->position.data(), msg->position.size()));
+      monitor_->environment().setState(
+          msg->name,
+          Eigen::Map<Eigen::VectorXd>(msg->position.data(), static_cast<Eigen::Index>(msg->position.size())));
       tesseract_scene_graph::SceneState state = monitor_->environment().getState();
 
       manager_->setCollisionObjectsTransform(state.link_transforms);
@@ -173,10 +177,10 @@ void ContactMonitor::computeCollisionReportThread()
     {
       contacts.flattenCopyResults(contacts_vector);
       contacts_msg.contacts.reserve(contacts_vector.size());
-      for (std::size_t i = 0; i < contacts_vector.size(); ++i)
+      for (const auto& contact : contacts_vector)
       {
         tesseract_msgs::msg::ContactResult contact_msg;
-        tesseract_rosutils::toMsg(contact_msg, contacts_vector[i], msg->header.stamp);
+        tesseract_rosutils::toMsg(contact_msg, contact, msg->header.stamp);
         contacts_msg.contacts.push_back(contact_msg);
       }
 
@@ -203,8 +207,9 @@ void ContactMonitor::callbackJointState(std::shared_ptr<sensor_msgs::msg::JointS
   current_joint_states_evt_.notify_all();
 }
 
-void ContactMonitor::callbackModifyTesseractEnv(tesseract_msgs::srv::ModifyEnvironment::Request::SharedPtr request,
-                                                tesseract_msgs::srv::ModifyEnvironment::Response::SharedPtr response)
+void ContactMonitor::callbackModifyTesseractEnv(
+    tesseract_msgs::srv::ModifyEnvironment::Request::SharedPtr request,    // NOLINT
+    tesseract_msgs::srv::ModifyEnvironment::Response::SharedPtr response)  // NOLINT
 {
   std::scoped_lock lock(modify_mutex_);
 
@@ -246,8 +251,8 @@ void ContactMonitor::callbackModifyTesseractEnv(tesseract_msgs::srv::ModifyEnvir
 }
 
 void ContactMonitor::callbackComputeContactResultVector(
-    tesseract_msgs::srv::ComputeContactResultVector::Request::SharedPtr request,
-    tesseract_msgs::srv::ComputeContactResultVector::Response::SharedPtr response)
+    tesseract_msgs::srv::ComputeContactResultVector::Request::SharedPtr request,    // NOLINT
+    tesseract_msgs::srv::ComputeContactResultVector::Response::SharedPtr response)  // NOLINT
 {
   thread_local tesseract_collision::ContactResultMap contact_results;
   thread_local tesseract_collision::ContactResultVector contacts_vector;
@@ -256,7 +261,8 @@ void ContactMonitor::callbackComputeContactResultVector(
 
   monitor_->environment().setState(
       request->joint_states.name,
-      Eigen::Map<Eigen::VectorXd>(request->joint_states.position.data(), request->joint_states.position.size()));
+      Eigen::Map<Eigen::VectorXd>(request->joint_states.position.data(),
+                                  static_cast<Eigen::Index>(request->joint_states.position.size())));
   tesseract_scene_graph::SceneState state = monitor_->environment().getState();
 
   // Limit the lock

tesseract_monitoring/src/contact_monitor_node.cpp

@@ -135,14 +135,14 @@ int main(int argc, char** argv)
 
   RCLCPP_INFO(node->get_logger(), "MONITORED_LINKS: %s", monitored_link_names_str.c_str());
 
-  int contact_test_type = node->declare_parameter("contact_test_type", 2);
+  auto contact_test_type = node->declare_parameter("contact_test_type", 2);
 
   if (contact_test_type < 0 || contact_test_type > 3)
   {
     RCLCPP_WARN(node->get_logger(), "Request type must be 0, 1, 2 or 3. Setting to 2(ALL)!");
     contact_test_type = 2;
   }
-  tesseract_collision::ContactTestType type = static_cast<tesseract_collision::ContactTestType>(contact_test_type);
+  auto type = static_cast<tesseract_collision::ContactTestType>(contact_test_type);
 
   tesseract_monitoring::ContactMonitor cm(monitor_namespace,
                                           std::move(env),

tesseract_monitoring/src/current_state_monitor.cpp

@@ -60,14 +60,15 @@ CurrentStateMonitor::CurrentStateMonitor(const tesseract_environment::Environmen
 
 CurrentStateMonitor::CurrentStateMonitor(const tesseract_environment::Environment::ConstPtr& env,
                                          rclcpp::Node::SharedPtr node)
-  : node_(node)
+  : node_(std::move(node))
   , env_(env)
   , env_state_(env->getState())
   , last_environment_revision_(env_->getRevision())
   , state_monitor_started_(false)
   , copy_dynamics_(false)
   , error_(std::numeric_limits<double>::epsilon())
   , tf_broadcaster_(node_)
+  , publish_tf_(true)
 {
 }
 
@@ -121,7 +122,7 @@ void CurrentStateMonitor::startStateMonitor(const std::string& joint_states_topi
       joint_state_subscriber_ = node_->create_subscription<sensor_msgs::msg::JointState>(
           joint_states_topic,
           rclcpp::SensorDataQoS(),
-          std::bind(&CurrentStateMonitor::jointStateCallback, this, std::placeholders::_1));
+          std::bind(&CurrentStateMonitor::jointStateCallback, this, std::placeholders::_1));  // NOLINT
     }
     state_monitor_started_ = true;
     monitor_start_time_ = node_->now();
@@ -149,7 +150,7 @@ std::string CurrentStateMonitor::getMonitoredTopic() const
   return "";
 }
 
-bool CurrentStateMonitor::isPassiveOrMimicDOF(const std::string& /*dof*/) const
+bool CurrentStateMonitor::isPassiveOrMimicDOF(const std::string& /*dof*/) const  // NOLINT
 {
   // TODO: Levi Need to implement
 
@@ -267,7 +268,7 @@ bool CurrentStateMonitor::haveCompleteState(const rclcpp::Duration& age, std::ve
   return result;
 }
 
-bool CurrentStateMonitor::waitForCurrentState(rclcpp::Time t, double wait_time) const
+bool CurrentStateMonitor::waitForCurrentState(const rclcpp::Time& t, double wait_time) const
 {
   rclcpp::Time start = rclcpp::Clock{ RCL_STEADY_TIME }.now();
   rclcpp::Duration elapsed(0, 0);
@@ -325,7 +326,7 @@ bool CurrentStateMonitor::waitForCompleteState(const std::string& manip, double
   return ok;
 }
 
-void CurrentStateMonitor::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr joint_state)
+void CurrentStateMonitor::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr joint_state)  // NOLINT
 {
   if (!env_->isInitialized())
     return;