From 403380774ebe5f4818e0e8d311ff4d0a3db8231f Mon Sep 17 00:00:00 2001
From: Roelof Oomen <rj.oomen@isohorti.com>
Date: Wed, 11 Oct 2023 14:12:58 +0200
Subject: [PATCH] - Implement plotting a trajectory with commands - Implement
 trajectory namespace option

---
 tesseract_msgs/msg/Trajectory.msg             |  2 +-
 .../include/tesseract_rosutils/plotting.h     |  6 ++-
 tesseract_rosutils/src/plotting.cpp           | 43 ++++++++++++++++++-
 3 files changed, 47 insertions(+), 4 deletions(-)

diff --git a/tesseract_msgs/msg/Trajectory.msg b/tesseract_msgs/msg/Trajectory.msg
index 6b002623..839915c4 100644
--- a/tesseract_msgs/msg/Trajectory.msg
+++ b/tesseract_msgs/msg/Trajectory.msg
@@ -11,7 +11,7 @@ string description
 # (Optional) Override the existing Tesseract Environment
 Environment environment
 
-# (Optional) Additional Commands to be applied to environment prior to trajectory visualization
+# (Optional) Additional Commands to be applied to environment prior to trajectory visualization (ignored if environment is provided)
 tesseract_msgs/EnvironmentCommand[] commands
 
 # (Required) Initial Environment Joint State
diff --git a/tesseract_rosutils/include/tesseract_rosutils/plotting.h b/tesseract_rosutils/include/tesseract_rosutils/plotting.h
index 1dad8ab7..b030d29b 100644
--- a/tesseract_rosutils/include/tesseract_rosutils/plotting.h
+++ b/tesseract_rosutils/include/tesseract_rosutils/plotting.h
@@ -67,9 +67,13 @@ class ROSPlotting : public tesseract_visualization::Visualization
                       const tesseract_planning::InstructionPoly& instruction,
                       std::string ns = "");
 
+  void plotTrajectory(const tesseract_environment::Commands& cmds,
+                      const tesseract_planning::InstructionPoly& instruction,
+                      std::string = "");
+
   void plotToolpath(const tesseract_environment::Environment& env,
                     const tesseract_planning::InstructionPoly& instruction,
-                    std::string ns);
+                    std::string ns = "");
 
   void plotMarker(const tesseract_visualization::Marker& marker, std::string ns = "") override;
 
diff --git a/tesseract_rosutils/src/plotting.cpp b/tesseract_rosutils/src/plotting.cpp
index 651328e4..557a35dd 100644
--- a/tesseract_rosutils/src/plotting.cpp
+++ b/tesseract_rosutils/src/plotting.cpp
@@ -112,9 +112,10 @@ void ROSPlotting::plotTrajectory(const tesseract_msgs::msg::Trajectory& traj, st
 
 void ROSPlotting::plotTrajectory(const tesseract_common::JointTrajectory& traj,
                                  const tesseract_scene_graph::StateSolver& /*state_solver*/,
-                                 std::string /*ns*/)
+                                 std::string ns)
 {
   tesseract_msgs::msg::Trajectory msg;
+  msg.ns = ns;
 
   if (!traj.empty())
   {
@@ -138,9 +139,10 @@ void ROSPlotting::plotTrajectory(const tesseract_common::JointTrajectory& traj,
 
 void ROSPlotting::plotTrajectory(const tesseract_environment::Environment& env,
                                  const tesseract_planning::InstructionPoly& instruction,
-                                 std::string /*ns*/)
+                                 std::string ns)
 {
   tesseract_msgs::msg::Trajectory msg;
+  msg.ns = ns;
 
   // Set tesseract state information
   toMsg(msg.environment, env);
@@ -168,6 +170,43 @@ void ROSPlotting::plotTrajectory(const tesseract_environment::Environment& env,
   plotTrajectory(msg);
 }
 
+void ROSPlotting::plotTrajectory(const tesseract_environment::Commands& cmds,
+                                 const tesseract_planning::InstructionPoly& instruction,
+                                 std::string ns)
+{
+  tesseract_msgs::msg::Trajectory msg;
+  msg.ns = ns;
+
+  // Set the commands message
+  std::vector<tesseract_msgs::msg::EnvironmentCommand> env_cmds;
+  tesseract_rosutils::toMsg(env_cmds, cmds, 0);
+  msg.commands = env_cmds;
+
+  // Convert to joint trajectory
+  assert(instruction.isCompositeInstruction());
+  const auto& ci = instruction.as<tesseract_planning::CompositeInstruction>();
+  tesseract_common::JointTrajectory traj = tesseract_planning::toJointTrajectory(ci);
+
+  if (!traj.empty())
+  {
+    // Set the initial state
+    for (std::size_t i = 0; i < traj[0].joint_names.size(); ++i)
+    {
+      tesseract_msgs::msg::StringDoublePair pair;
+      pair.first = traj[0].joint_names[i];
+      pair.second = traj[0].position[static_cast<Eigen::Index>(i)];
+      msg.initial_state.push_back(pair);
+    }
+  }
+
+  // Set the joint trajectory message
+  tesseract_msgs::msg::JointTrajectory traj_msg;
+  toMsg(traj_msg, traj);
+  msg.joint_trajectories.push_back(traj_msg);
+
+  plotTrajectory(msg);
+}
+
 void ROSPlotting::plotMarker(const tesseract_visualization::Marker& marker, std::string ns)
 {
   switch (marker.getType())