Merge pull request #41 from rjoomen/master

tesseract_ros 0.16.2 parity

.github/workflows/clang_tidy.yml

@@ -7,25 +7,27 @@ on:
   pull_request:
   schedule:
     - cron: '0 5 * * *'
+  workflow_dispatch:
 
 jobs:
   industrial_ci:
     name: ClangTidy
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     env:
       CI_NAME: Clang-Tidy
       OS_NAME: ubuntu
-      OS_CODE_NAME: focal
-      ROS_DISTRO: foxy
+      OS_CODE_NAME: jammy
+      ROS_DISTRO: humble
       ROS_REPO: main
+      ADDITIONAL_DEBS: clang-tidy
       UPSTREAM_WORKSPACE: 'dependencies.repos'
-      ROSDEP_SKIP_KEYS: "catkin taskflow"
-      DOCKER_IMAGE: "ros:foxy"
-      AFTER_INSTALL_UPSTREAM_DEPENDENCIES: "sh .ci-install-taskflow.sh"
-      CCACHE_DIR: "/home/runner/work/tesseract_ros/tesseract_ros/Clang-Tidy/.ccache"
+      ROSDEP_SKIP_KEYS: "catkin taskflow orocos_kdl fcl"
+      DOCKER_IMAGE: "ros:humble"
       NOT_TEST_BUILD: true
+      CCACHE_DIR: "/home/runner/work/tesseract_ros/tesseract_ros/Clang-Tidy/.ccache"
       UPSTREAM_CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"
       TARGET_CMAKE_ARGS: "-DTESSERACT_ENABLE_CLANG_TIDY=ON -DTESSERACT_ENABLE_TESTING=ON"
+      CXXFLAGS: "-std=c++17"
     steps:
       - uses: actions/checkout@v1
 

.run-clang-format

@@ -1,2 +1,2 @@
 #!/bin/bash
-find . -type d \( -name bullet3_ros -o -name fcl_ros -o -name libccd_ros -o -name gtest_ros -o -name tesseract_ext \) -prune -o -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-10 -style=file -i {} \;
+find . -type d \( -name bullet3_ros -o -name fcl_ros -o -name libccd_ros -o -name gtest_ros -o -name tesseract_ext \) -prune -o -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format-8 -style=file -i {} \;

README.md

@@ -17,8 +17,7 @@ Lint  (Clang Tidy)   | [![Build Status](https://github.com/ros-industrial-consor
 
 ## Tesseract ROS Packages
 
-* **tesseract_examples** – This package contains examples using tesseract and tesseract_ros for motion planning and collision checking.
-* **tesseract_plugins** – This contains plugins for collision and kinematics which are automatically loaded by the monitors.
+* **tesseract_ros_examples** – This package contains ROS examples using tesseract and tesseract_ros for motion planning and collision checking.
 * **tesseract_rosutils** – This package contains the utilities like converting from ROS message types to native Tesseract types and the reverse.
 * **tesseract_msgs** – This package contains the ROS message types used by Tesseract ROS.
 * **tesseract_rviz** – This package contains the ROS visualization plugins for Rviz to visualize Tesseract. All of the features have been composed in libraries to enable to the ability to create custom displays quickly.

dependencies.repos

@@ -6,19 +6,19 @@ repositories:
   tesseract:
     type: git
     url: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.15.3
+    version: 0.16.2
   trajopt:
     type: git
     url: https://github.com/tesseract-robotics/trajopt.git
-    version: 0.4.2
+    version: 0.5.1
   tesseract_planning:
     type: git
     url: https://github.com/tesseract-robotics/tesseract_planning.git
-    version: 0.15.5
+    version: 0.16.2
   tesseract_qt:
     type: git
     url: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.15.2
+    version: 0.16.2
   descartes_light:
     type: git
     url: https://github.com/swri-robotics/descartes_light.git

dependencies_with_ext.rosinstall

@@ -9,19 +9,19 @@
 - git:
     local-name: tesseract
     uri: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.15.0
+    version: 0.16.2
 - git:
     local-name: trajopt
     uri: https://github.com/tesseract-robotics/trajopt_ros.git
-    version: 0.4.0
+    version: 0.5.0
 - git:
     local-name: tesseract_planning
     uri: https://github.com/tesseract-robotics/tesseract_planning.git
-    version: 0.15.0
+    version: 0.16.2
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.15.0
+    version: 0.16.2
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git

tesseract_monitoring/CHANGELOG.rst

@@ -0,0 +1,133 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package tesseract_monitoring
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.16.2 (2023-04-28)
+-------------------
+
+0.16.1 (2023-04-11)
+-------------------
+
+0.16.0 (2023-04-10)
+-------------------
+* Update to leverge ContactResultMap class (`#205 <https://github.com/tesseract-robotics/tesseract_ros/issues/205>`_)
+* Contributors: Levi Armstrong
+
+0.15.2 (2023-03-14)
+-------------------
+
+0.15.1 (2023-03-05)
+-------------------
+
+0.15.0 (2023-03-04)
+-------------------
+
+0.14.0 (2022-10-23)
+-------------------
+* Remove use of deprecated items
+* Contributors: Levi Armstrong
+
+0.6.0 (2022-08-25)
+------------------
+
+0.5.1 (2022-06-21)
+------------------
+
+0.5.0 (2022-05-17)
+------------------
+
+0.4.4 (2022-05-13)
+------------------
+* Add new RViz plugins using Tesseract widgets (`#152 <https://github.com/tesseract-robotics/tesseract_ros/issues/152>`_)
+* Contributors: Levi Armstrong
+
+0.4.3 (2022-05-03)
+------------------
+
+0.4.2 (2022-04-25)
+------------------
+
+0.4.1 (2022-04-13)
+------------------
+* Fix handling of wait(0) in environment monitor interface
+* Contributors: Levi Armstrong
+
+0.4.0 (2022-04-08)
+------------------
+* Update to use monitor interface and clean up environment monitor
+* Contributors: Levi Armstrong
+
+0.3.3 (2022-02-22)
+------------------
+
+0.3.2 (2022-01-21)
+------------------
+
+0.3.1 (2021-12-16)
+------------------
+* Add ability to disable links when launching contact monitor
+* Contributors: Levi Armstrong
+
+0.3.0 (2021-12-06)
+------------------
+* Support moving AllowedCollisionMatrix into tesseract_common namespace
+* Contributors: Matthew Powelson
+
+0.2.2 (2021-11-30)
+------------------
+
+0.2.1 (2021-11-30)
+------------------
+* Cleanup CMakeLists.txt
+* Contributors: Levi Armstrong
+
+0.2.0 (2021-11-04)
+------------------
+* Update due to changes with contact manager plugins
+* Update to Joint and Kinematic group (`#125 <https://github.com/tesseract-robotics/tesseract_ros/issues/125>`_)
+* Rename tesseract_monitor_interface to environment_monitor_interface
+* Clean up environment monitor and interface
+* Update new tesseract_srdf package
+* Clang format
+* Change TesseractState.msg to EnvironmentState.msg
+* Update to latest tesseract_environment changes and fix online planning example
+* Update cmake_common_scripts to ros_industrial_cmake_boilerplate
+* Update to leverage new visualizaton interface
+* Move all packages out of tesseract_ros sub directory
+* Contributors: Levi Armstrong, Levi-Armstrong, Matthew Powelson
+
+0.1.0 (2020-12-02)
+------------------
+* WIP: Move ROS package into sub folder
+* Semi-Isolate Tesseract Kinematics
+* Isolate tesseract_collision namespace
+* Switch to using built in Collision Shapes
+* Clang formatting changes
+* Added service server to tesseract environment monitor for updating the environment
+* Merge branch 'kinetic-devel' into acm_fixes
+* Added installation of tesseract_monitoring launch files to CMakeLists
+* Fix formatting using clang
+* Fix warnings in unit tests
+* Add additional compiler warning options
+* Implement topic subscriber for updating collision monitor environment
+* Implement synchronous "compute_contact_reports" service in contact_monitor.cpp
+* Fixed typo 'constacts' in ContactResultVector.msg
+* Merge pull request `#41 <https://github.com/tesseract-robotics/tesseract_ros/issues/41>`_ from Levi-Armstrong/issue/FixMultiLayerCompoundShape
+  Fix use of multi layer compound shape
+  Fix/add cmake install commands
+* Fix cmake install commands
+* Merge pull request `#40 <https://github.com/tesseract-robotics/tesseract_ros/issues/40>`_ from Levi-Armstrong/feature/RemoveContactRequestStruct
+  Refractor out ContactRequest type
+* Refractor out ContactRequest type
+* Merge pull request `#26 <https://github.com/tesseract-robotics/tesseract_ros/issues/26>`_ from Levi-Armstrong/issue/FixContactMonitor
+  Update contact monitor to use the latest version
+* Fix the contact monitor to use the new contact managers
+* Merge pull request `#20 <https://github.com/tesseract-robotics/tesseract_ros/issues/20>`_ from Levi-Armstrong/feature/Isometry3d
+  switch from using affine3d to isometry3d
+* switch from using affine3d to isometry3d
+* Merge pull request `#15 <https://github.com/tesseract-robotics/tesseract_ros/issues/15>`_ from Levi-Armstrong/feature/largeDataSetTest
+  Restructure Collision Checking for Performance Improvements
+* Run clang-format
+* Restructure Collision Checking for Performance Improvements
+* Move tesseract into its own repository
+* Contributors: Alessio Rocchi, John Wason, Levi, Levi Armstrong, mripperger

tesseract_monitoring/include/tesseract_monitoring/contact_monitor.h

@@ -81,7 +81,7 @@ class ContactMonitor
    * @brief Start monitoring an environment for applying changes to this environment
    * @param topic The topic to monitor for environment changes
    */
-  void startMonitoringEnvironment(const std::string& monitored_namepsace);
+  void startMonitoringEnvironment(const std::string& monitored_namespace);
 
   /**
    * @brief Start publishing the contact markers

tesseract_monitoring/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>tesseract_monitoring</name>
-  <version>0.14.0</version>
+  <version>0.16.2</version>
   <description>The tesseract_monitoring package</description>
   <maintainer email="[email protected]">Levi Armstrong</maintainer>
   <license>Apache 2.0</license>

tesseract_monitoring/src/contact_monitor.cpp

@@ -92,9 +92,9 @@ ContactMonitor::~ContactMonitor() { current_joint_states_evt_.notify_all(); }
 
 void ContactMonitor::startPublishingEnvironment() { monitor_->startPublishingEnvironment(); }
 
-void ContactMonitor::startMonitoringEnvironment(const std::string& monitored_namepsace)
+void ContactMonitor::startMonitoringEnvironment(const std::string& monitored_namespace)
 {
-  monitor_->startMonitoringEnvironment(monitored_namepsace);
+  monitor_->startMonitoringEnvironment(monitored_namespace);
 }
 
 void ContactMonitor::startPublishingMarkers()
@@ -116,6 +116,7 @@ void ContactMonitor::computeCollisionReportThread()
   {
     std::shared_ptr<sensor_msgs::msg::JointState> msg = nullptr;
     tesseract_collision::ContactResultMap contacts;
+    tesseract_collision::ContactResultVector contacts_vector;
     tesseract_msgs::msg::ContactResultVector contacts_msg;
     std::string root_link;
     // Limit the lock
@@ -158,6 +159,7 @@ void ContactMonitor::computeCollisionReportThread()
       current_joint_states_.reset();
 
       contacts.clear();
+      contacts_vector.clear();
       contacts_msg.contacts.clear();
 
       monitor_->environment().setState(msg->name,
@@ -170,8 +172,7 @@ void ContactMonitor::computeCollisionReportThread()
 
     if (!contacts.empty())
     {
-      tesseract_collision::ContactResultVector contacts_vector;
-      tesseract_collision::flattenMoveResults(std::move(contacts), contacts_vector);
+      contacts.flattenCopyResults(contacts_vector);
       contacts_msg.contacts.reserve(contacts_vector.size());
       for (std::size_t i = 0; i < contacts_vector.size(); ++i)
       {
@@ -249,7 +250,10 @@ void ContactMonitor::callbackComputeContactResultVector(
     tesseract_msgs::srv::ComputeContactResultVector::Request::SharedPtr request,
     tesseract_msgs::srv::ComputeContactResultVector::Response::SharedPtr response)
 {
-  tesseract_collision::ContactResultMap contact_results;
+  thread_local tesseract_collision::ContactResultMap contact_results;
+  thread_local tesseract_collision::ContactResultVector contacts_vector;
+  contact_results.clear();
+  contacts_vector.clear();
 
   monitor_->environment().setState(
       request->joint_states.name,
@@ -263,8 +267,7 @@ void ContactMonitor::callbackComputeContactResultVector(
     manager_->contactTest(contact_results, type_);
   }
 
-  tesseract_collision::ContactResultVector contacts_vector;
-  tesseract_collision::flattenMoveResults(std::move(contact_results), contacts_vector);
+  contact_results.flattenCopyResults(contacts_vector);
   response->collision_result.contacts.reserve(contacts_vector.size());
   for (const auto& contact : contacts_vector)
   {

tesseract_monitoring/src/environment_monitor_interface.cpp

@@ -58,7 +58,11 @@ typename SrvType::Response::SharedPtr call_service(const std::string& name,
   }
 
   rclcpp::executors::SingleThreadedExecutor exec;
-  // exec.add_callback_group(cbg, node.get_node_base_interface());
+#if __has_include(<rclcpp/version.h>)  // ROS 2 Humble
+  exec.add_callback_group(cbg, node.get_node_base_interface());
+#else  // ROS 2 Foxy
+  exec.add_node(node.get_node_base_interface());
+#endif
 
   auto future = client->async_send_request(request);
   auto retcode = exec.spin_until_future_complete(future, timeout);
@@ -72,13 +76,16 @@ typename SrvType::Response::SharedPtr call_service(const std::string& name,
 }
 
 ROSEnvironmentMonitorInterface::ROSEnvironmentMonitorInterface(rclcpp::Node::SharedPtr node, const std::string env_name)
-  : EnvironmentMonitorInterface(std::move(env_name))
-  , node_{ node }
-  , logger_{ node_->get_logger().get_child(env_name + "_env_monitor") }
-  , env_name_{ env_name }
+  : EnvironmentMonitorInterface(std::move(env_name)), node_
 {
-  // callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  node
 }
+#if __has_include(<rclcpp/version.h>)  // ROS 2 Humble
+, callback_group_ { node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false) }
+#else  // ROS 2 Foxy
+, callback_group_ { node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive) }
+#endif
+, logger_{ node_->get_logger().get_child(env_name + "_env_monitor") }, env_name_{ env_name } {}
 
 bool ROSEnvironmentMonitorInterface::wait(std::chrono::duration<double> duration) const
 {
@@ -113,7 +120,7 @@ bool ROSEnvironmentMonitorInterface::wait(std::chrono::duration<double> duration
 bool ROSEnvironmentMonitorInterface::waitForNamespace(const std::string& monitor_namespace,
                                                       std::chrono::duration<double> duration) const
 {
-  std::string service_name = R"(/)" + monitor_namespace + DEFAULT_GET_ENVIRONMENT_INFORMATION_SERVICE;
+  std::string srv_name = R"(/)" + monitor_namespace + DEFAULT_GET_ENVIRONMENT_INFORMATION_SERVICE;
   const auto start_time = rclcpp::Clock().now();
   rclcpp::Duration wall_timeout(duration);
   if (std::chrono::duration_cast<std::chrono::seconds>(duration).count() == 0)
@@ -126,8 +133,8 @@ bool ROSEnvironmentMonitorInterface::waitForNamespace(const std::string& monitor
     try
     {
       auto res =
-          call_service<tesseract_msgs::srv::GetEnvironmentInformation>(service_name, req, *node_, callback_group_, 5s);
-      if (res->success)
+          call_service<tesseract_msgs::srv::GetEnvironmentInformation>(srv_name, req, *node_, callback_group_, 5s);
+      if (res && res->success)
         return true;
     }
     catch (std::runtime_error& ex)
@@ -263,8 +270,8 @@ bool ROSEnvironmentMonitorInterface::sendCommands(
 
   try
   {
-    auto response = call_service<tesseract_msgs::srv::ModifyEnvironment>(
-        ns + DEFAULT_MODIFY_ENVIRONMENT_SERVICE, req, *node_, callback_group_, 10s);
+    const std::string srv_name = R"(/)" + ns + DEFAULT_MODIFY_ENVIRONMENT_SERVICE;
+    auto response = call_service<tesseract_msgs::srv::ModifyEnvironment>(srv_name, req, *node_, callback_group_, 10s);
     if (!response || !response->success)
     {
       RCLCPP_ERROR_STREAM(logger_, "sendCommands (" + ns + "): Failed to update monitored environment!");