- Updates to support visualizing online planning example

- 0.26.0 - Update changelogs - Update rosinstall
tesseract-robotics · Nov 19, 2024 · 77bfeb2 · 77bfeb2
1 parent 3b7e8b0
commit 77bfeb2
Show file tree

Hide file tree

Showing 21 changed files with 115 additions and 24 deletions.
diff --git a/dependencies.repos b/dependencies.repos
@@ -6,19 +6,19 @@ repositories:
   tesseract:
     type: git
     url: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.25.0
+    version: 0.26.0
   trajopt:
     type: git
     url: https://github.com/tesseract-robotics/trajopt.git
-    version: 0.25.1
+    version: 0.26.0
   tesseract_planning:
     type: git
     url: https://github.com/tesseract-robotics/tesseract_planning.git
-    version: 0.25.1
+    version: 0.26.0
   tesseract_qt:
     type: git
     url: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.25.0
+    version: 0.26.0
   descartes_light:
     type: git
     url: https://github.com/swri-robotics/descartes_light.git

diff --git a/dependencies_with_ext.repos b/dependencies_with_ext.repos
@@ -10,19 +10,19 @@ repositories:
   tesseract:
     type: git
     url: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.25.0
+    version: 0.26.0
   trajopt:
     type: git
     url: https://github.com/tesseract-robotics/trajopt.git
-    version: 0.25.1
+    version: 0.26.0
   tesseract_planning:
     type: git
     url: https://github.com/tesseract-robotics/tesseract_planning.git
-    version: 0.25.1
+    version: 0.26.0
   tesseract_qt:
     type: git
     url: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.25.0
+    version: 0.26.0
   descartes_light:
     type: git
     url: https://github.com/swri-robotics/descartes_light.git

diff --git a/tesseract_monitoring/CHANGELOG.rst b/tesseract_monitoring/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package tesseract_monitoring
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.26.0 (2024-11-19)
+-------------------
+
 0.25.0 (2024-09-30)
 -------------------
 

diff --git a/tesseract_monitoring/package.xml b/tesseract_monitoring/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>tesseract_monitoring</name>
-  <version>0.25.0</version>
+  <version>0.26.0</version>
   <description>The tesseract_monitoring package</description>
   <maintainer email="[email protected]">Levi Armstrong</maintainer>
   <license>Apache 2.0</license>

diff --git a/tesseract_msgs/CHANGELOG.rst b/tesseract_msgs/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package tesseract_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.26.0 (2024-11-19)
+-------------------
+
 0.25.0 (2024-09-30)
 -------------------
 * Update to support Geometry Type CompoundMesh

diff --git a/tesseract_msgs/msg/JointTrajectory.msg b/tesseract_msgs/msg/JointTrajectory.msg
@@ -1,2 +1,3 @@
 tesseract_msgs/JointState[] states
 string description # A description of the trajectory
+string uuid # Unique ID
diff --git a/tesseract_msgs/msg/Trajectory.msg b/tesseract_msgs/msg/Trajectory.msg
@@ -25,4 +25,6 @@ tesseract_msgs/StringDoublePair[] initial_state
 string instructions
 
 # If instructions are provided this will be ignored
+string joint_trajectories_uuid
+string joint_trajectories_description
 tesseract_msgs/JointTrajectory[] joint_trajectories
diff --git a/tesseract_msgs/package.xml b/tesseract_msgs/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>tesseract_msgs</name>
-  <version>0.25.0</version>
+  <version>0.26.0</version>
   <description>The tesseract_msgs package</description>
   <maintainer email="[email protected]">Levi Armstrong</maintainer>
   <license>Apache 2.0</license>

diff --git a/tesseract_planning_server/CHANGELOG.rst b/tesseract_planning_server/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package tesseract_planning_server
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.26.0 (2024-11-19)
+-------------------
+* Update to latest tesseract
+* Contributors: Levi Armstrong, Roelof Oomen
+
 0.25.0 (2024-09-30)
 -------------------
 * Change timer to stopwatch

diff --git a/tesseract_planning_server/package.xml b/tesseract_planning_server/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>tesseract_planning_server</name>
-  <version>0.25.0</version>
+  <version>0.26.0</version>
   <description>Tesseract ROS Planning Server</description>
   <maintainer email="[email protected]">Levi Armstrong</maintainer>
   <license>Apache 2.0</license>

diff --git a/tesseract_qt_ros/CHANGELOG.rst b/tesseract_qt_ros/CHANGELOG.rst
@@ -2,6 +2,23 @@
 Changelog for package tesseract_qt_ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.26.0 (2024-11-19)
+-------------------
+
+0.25.0 (2024-09-29)
+-------------------
+
+0.24.0 (2024-08-15)
+-------------------
+
+0.23.0 (2024-07-25)
+-------------------
+
+0.22.0 (2024-06-11)
+-------------------
+* Leverage forward declarations (`#233 <https://github.com/tesseract-robotics/tesseract_ros/issues/233>`_)
+* Contributors: Levi Armstrong
+
 0.21.0 (2023-11-10)
 -------------------
 
@@ -16,6 +33,8 @@ Changelog for package tesseract_qt_ros
 
 0.18.1 (2023-07-10)
 -------------------
+* Add Tesseract Studio ROS executable
+* Contributors: Levi Armstrong
 
 0.18.0 (2023-07-03)
 -------------------

diff --git a/tesseract_qt_ros/package.xml b/tesseract_qt_ros/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>tesseract_qt_ros</name>
-  <version>0.22.0</version>
+  <version>0.26.0</version>
   <description>The tesseract_qt_ros package</description>
   <maintainer email="[email protected]">Levi Armstrong</maintainer>
   <license>LGPL3.0</license>

diff --git a/tesseract_ros_examples/CHANGELOG.rst b/tesseract_ros_examples/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package tesseract_ros_examples
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.26.0 (2024-11-19)
+-------------------
+
 0.25.0 (2024-09-30)
 -------------------
 

diff --git a/tesseract_ros_examples/package.xml b/tesseract_ros_examples/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>tesseract_ros_examples</name>
-  <version>0.25.0</version>
+  <version>0.26.0</version>
   <description>This package contains examples related to this trajopt_ros repository.</description>
   <maintainer email="[email protected]">Levi Armstrong</maintainer>
   <license>Apache 2.0</license>

diff --git a/tesseract_rosutils/CHANGELOG.rst b/tesseract_rosutils/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package tesseract_rosutils
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.26.0 (2024-11-19)
+-------------------
+* Update to latest tesseract
+* Contributors: Levi Armstrong, Roelof Oomen
+
 0.25.0 (2024-09-30)
 -------------------
 * Add plotToolpath for ToolPath

diff --git a/tesseract_rosutils/package.xml b/tesseract_rosutils/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>tesseract_rosutils</name>
-  <version>0.25.0</version>
+  <version>0.26.0</version>
   <description>Tesseract ROS Utilities</description>
   <maintainer email="[email protected]">Levi Armstrong</maintainer>
   <license>Apache 2.0</license>

diff --git a/tesseract_rosutils/src/plotting.cpp b/tesseract_rosutils/src/plotting.cpp
@@ -27,12 +27,15 @@
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <thread>
 #include <Eigen/Geometry>
+#include <boost/uuid/random_generator.hpp>
+#include <boost/uuid/uuid_io.hpp>
 #include <rclcpp/rclcpp.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_rosutils/plotting.h>
 #include <tesseract_rosutils/utils.h>
 
+#include <tesseract_common/serialization.h>
 #include <tesseract_command_language/utils.h>
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_motion_planners/core/utils.h>
@@ -128,6 +131,7 @@ void ROSPlotting::plotTrajectory(const tesseract_common::JointTrajectory& traj,
 {
   tesseract_msgs::msg::Trajectory msg;
   msg.ns = std::move(ns);
+  msg.joint_trajectories_uuid = boost::uuids::to_string(traj.uuid);
   msg.description = std::move(description);
 
   if (!traj.empty())
@@ -167,6 +171,7 @@ void ROSPlotting::plotTrajectories(const tesseract_environment::Environment& env
 {
   tesseract_msgs::msg::Trajectory msg;
   msg.ns = std::move(ns);
+  msg.joint_trajectories_uuid = boost::uuids::to_string(boost::uuids::random_generator()());
   msg.description = std::move(description);
 
   // Set tesseract state information
@@ -182,6 +187,10 @@ void ROSPlotting::plotTrajectories(const tesseract_environment::Environment& env
     msg.initial_state.push_back(pair_msg);
   }
 
+  msg.instructions =
+      tesseract_common::Serialization::toArchiveStringXML<tesseract_planning::InstructionPoly>(instructions[0]);
+  //.as<tesseract_planning::CompositeInstruction>()
+
   // Convert to joint trajectories
   for (const auto& instruction : instructions)
   {
@@ -204,6 +213,7 @@ void ROSPlotting::plotTrajectory(const tesseract_environment::Commands& cmds,
 {
   tesseract_msgs::msg::Trajectory msg;
   msg.ns = std::move(ns);
+  msg.joint_trajectories_uuid = boost::uuids::to_string(instruction.getUUID());
   msg.description = std::move(description);
 
   // Set the commands message

diff --git a/tesseract_rosutils/src/utils.cpp b/tesseract_rosutils/src/utils.cpp
@@ -1670,8 +1670,8 @@ void toMsg(const tesseract_msgs::msg::EnvironmentState::SharedPtr& state_msg,
 
 void toMsg(tesseract_msgs::msg::JointTrajectory& traj_msg, const tesseract_common::JointTrajectory& traj)
 {
+  traj_msg.uuid = boost::uuids::to_string(traj.uuid);
   traj_msg.description = traj.description;
-
   for (const auto& js : traj)
   {
     assert(js.joint_names.size() == static_cast<unsigned>(js.position.size()));
@@ -1699,6 +1699,7 @@ void toMsg(tesseract_msgs::msg::JointTrajectory& traj_msg, const tesseract_commo
 tesseract_common::JointTrajectory fromMsg(const tesseract_msgs::msg::JointTrajectory& traj_msg)
 {
   tesseract_common::JointTrajectory trajectory;
+  trajectory.uuid = boost::lexical_cast<boost::uuids::uuid>(traj_msg.uuid);
   trajectory.description = traj_msg.description;
   for (const auto& js_msg : traj_msg.states)
   {

diff --git a/tesseract_rviz/CHANGELOG.rst b/tesseract_rviz/CHANGELOG.rst
@@ -2,10 +2,13 @@
 Changelog for package tesseract_rviz
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.26.0 (2024-11-19)
+-------------------
+
 0.25.0 (2024-09-30)
 -------------------
 * Update to support Geometry Type CompoundMesh
-* Contributors: Roelof Oomen
+* Contributors: Levi Armstrong, Roelof Oomen
 
 0.23.0 (2024-08-21)
 -------------------

diff --git a/tesseract_rviz/package.xml b/tesseract_rviz/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>tesseract_rviz</name>
-  <version>0.25.0</version>
+  <version>0.26.0</version>
   <description>The tesseract_rviz package</description>
   <maintainer email="[email protected]">Levi Armstrong</maintainer>
   <license>BSD</license>

diff --git a/tesseract_rviz/src/joint_trajectory_monitor_properties.cpp b/tesseract_rviz/src/joint_trajectory_monitor_properties.cpp
@@ -18,12 +18,17 @@
 
 #include <unordered_map>
 #include <thread>
+#include <boost/lexical_cast.hpp>
+#include <boost/uuid/uuid_io.hpp>
 
 #include <QApplication>
 
+#include <tesseract_common/serialization.h>
 #include <tesseract_environment/environment.h>
 #include <tesseract_environment/command.h>
 #include <tesseract_environment/commands.h>
+#include <tesseract_command_language/composite_instruction.h>
+#include <tesseract_command_language/utils.h>
 
 namespace tesseract_rviz
 {
@@ -62,6 +67,8 @@ struct JointTrajectoryMonitorProperties::Implementation
 
     tesseract_common::JointTrajectorySet trajectory_set(initial_state);
     tesseract_common::JointTrajectory joint_trajectory = tesseract_rosutils::fromMsg(*msg);
+    trajectory_set.setUUID(joint_trajectory.uuid);
+    trajectory_set.setDescription(joint_trajectory.description);
     trajectory_set.appendJointTrajectory(joint_trajectory);
     tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set);
     QApplication::sendEvent(qApp, &event);
@@ -98,15 +105,44 @@ struct JointTrajectoryMonitorProperties::Implementation
       if (!msg->ns.empty())
         trajectory_set.setNamespace(msg->ns);
 
-      for (const auto& joint_trajectory_msg : msg->joint_trajectories)
+      if (!msg->instructions.empty())
       {
-        tesseract_common::JointTrajectory joint_trajectory = tesseract_rosutils::fromMsg(joint_trajectory_msg);
-        trajectory_set.appendJointTrajectory(joint_trajectory);
+        auto ci = tesseract_common::Serialization::fromArchiveStringXML<tesseract_planning::CompositeInstruction>(
+            msg->instructions);
+        trajectory_set.setUUID(ci.getUUID());
+        trajectory_set.setDescription(ci.getDescription());
+        if (!ci.empty() && ci.front().isCompositeInstruction())
+        {
+          for (const auto& entry : ci)
+          {
+            const auto& sub_ci = entry.as<tesseract_planning::CompositeInstruction>();
+            tesseract_common::JointTrajectory joint_trajectory = tesseract_planning::toJointTrajectory(sub_ci);
+            trajectory_set.appendJointTrajectory(joint_trajectory);
+          }
+
+          tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set);
+          QApplication::sendEvent(qApp, &event);
+        }
+        else
+        {
+          tesseract_common::JointTrajectory joint_trajectory = tesseract_planning::toJointTrajectory(ci);
+          trajectory_set.appendJointTrajectory(joint_trajectory);
+          tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set);
+          QApplication::sendEvent(qApp, &event);
+        }
+      }
+      else
+      {
+        trajectory_set.setUUID(boost::lexical_cast<boost::uuids::uuid>(msg->joint_trajectories_uuid));
+        trajectory_set.setDescription(msg->joint_trajectories_description);
+        for (const auto& joint_trajectory_msg : msg->joint_trajectories)
+        {
+          tesseract_common::JointTrajectory joint_trajectory = tesseract_rosutils::fromMsg(joint_trajectory_msg);
+          trajectory_set.appendJointTrajectory(joint_trajectory);
+          tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set);
+          QApplication::sendEvent(qApp, &event);
+        }
       }
-
-      trajectory_set.setDescription(msg->description);
-      tesseract_gui::events::JointTrajectoryAdd event(component_info, trajectory_set);
-      QApplication::sendEvent(qApp, &event);
     }
     catch (...)
     {