Add missing floating joint components

dependencies.rosinstall

@@ -5,19 +5,19 @@
 - git:
     local-name: tesseract
     uri: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.28.0
+    version: 0.28.4
 - git:
     local-name: trajopt
     uri: https://github.com/tesseract-robotics/trajopt.git
-    version: 0.28.0
+    version: 0.28.1
 - git:
     local-name: tesseract_planning
     uri: https://github.com/tesseract-robotics/tesseract_planning.git
     version: 0.28.1
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.28.0
+    version: 0.28.1
 - git:
     local-name: descartes_light
     uri: 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.28.0
+    version: 0.28.4
 - git:
     local-name: trajopt
     uri: https://github.com/tesseract-robotics/trajopt_ros.git
-    version: 0.28.0
+    version: 0.28.1
 - git:
     local-name: tesseract_planning
     uri: https://github.com/tesseract-robotics/tesseract_planning.git
     version: 0.28.1
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.28.0
+    version: 0.28.1
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git

tesseract_monitoring/include/tesseract_monitoring/environment_monitor_interface.h

@@ -75,21 +75,33 @@ class ROSEnvironmentMonitorInterface : public tesseract_environment::Environment
   tesseract_scene_graph::SceneState getEnvironmentState(const std::string& monitor_namespace) const override final;
 
   bool setEnvironmentState(const std::string& monitor_namespace,
-                           const std::unordered_map<std::string, double>& joints) const override final;
+                           const std::unordered_map<std::string, double>& joints,
+                           const tesseract_common::TransformMap& floating_joints = {}) const override final;
   bool setEnvironmentState(const std::string& monitor_namespace,
                            const std::vector<std::string>& joint_names,
-                           const std::vector<double>& joint_values) const override final;
+                           const std::vector<double>& joint_values,
+                           const tesseract_common::TransformMap& floating_joints = {}) const override final;
   bool setEnvironmentState(const std::string& monitor_namespace,
                            const std::vector<std::string>& joint_names,
-                           const Eigen::Ref<const Eigen::VectorXd>& joint_values) const override final;
+                           const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                           const tesseract_common::TransformMap& floating_joints = {}) const override final;
+  bool setEnvironmentState(const std::string& monitor_namespace,
+                           const tesseract_common::TransformMap& floating_joints = {}) const override final;
 
   std::vector<std::string>
-  setEnvironmentState(const std::unordered_map<std::string, double>& joints) const override final;
-  std::vector<std::string> setEnvironmentState(const std::vector<std::string>& joint_names,
-                                               const std::vector<double>& joint_values) const override final;
+  setEnvironmentState(const std::unordered_map<std::string, double>& joints,
+                      const tesseract_common::TransformMap& floating_joints = {}) const override final;
+  std::vector<std::string>
+  setEnvironmentState(const std::vector<std::string>& joint_names,
+                      const std::vector<double>& joint_values,
+                      const tesseract_common::TransformMap& floating_joints = {}) const override final;
   std::vector<std::string>
   setEnvironmentState(const std::vector<std::string>& joint_names,
-                      const Eigen::Ref<const Eigen::VectorXd>& joint_values) const override final;
+                      const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                      const tesseract_common::TransformMap& floating_joints = {}) const override final;
+
+  std::vector<std::string>
+  setEnvironmentState(const tesseract_common::TransformMap& floating_joints = {}) const override final;
 
   std::unique_ptr<tesseract_environment::Environment>
   getEnvironment(const std::string& monitor_namespace) const override final;

tesseract_monitoring/src/environment_monitor.cpp

@@ -690,11 +690,12 @@ struct ROSEnvironmentMonitor::Implementation
       }
     }
 
-    if (!tesseract_rosutils::isMsgEmpty(env_msg->joint_state))
+    if (!tesseract_rosutils::isMsgEmpty(env_msg->joint_state) ||
+        !tesseract_rosutils::isMsgEmpty(env_msg->floating_joint_states))
     {
       if (last_robot_motion_time != env_msg->joint_state.header.stamp)
       {
-        tesseract_rosutils::processMsg(*parent->env_, env_msg->joint_state);
+        tesseract_rosutils::processMsg(*parent->env_, env_msg->joint_state, env_msg->floating_joint_states);
         last_robot_motion_time = env_msg->joint_state.header.stamp;
       }
     }
@@ -875,6 +876,12 @@ struct ROSEnvironmentMonitor::Implementation
         res.success = false;
         return false;
       }
+
+      if (!tesseract_rosutils::toMsg(res.floating_joint_states, state.floating_joints))
+      {
+        res.success = false;
+        return false;
+      }
     }
 
     res.success = true;
@@ -922,15 +929,14 @@ struct ROSEnvironmentMonitor::Implementation
         filtered_commands.push_back(cmd);
     }
 
-    std::string old_scene_name;
     if (!filtered_commands.empty())
       result = tesseract_rosutils::processMsg(env, filtered_commands);
 
     if (result)
     {
       for (const auto& cmd : update_joint_state_commands)
       {
-        if (tesseract_rosutils::processMsg(env, cmd.joint_state))
+        if (tesseract_rosutils::processMsg(env, cmd.joint_state, cmd.floating_joint_states))
         {
           last_robot_motion_time = ros::Time::now();
         }

tesseract_monitoring/src/environment_monitor_interface.cpp

@@ -245,24 +245,28 @@ ROSEnvironmentMonitorInterface::getEnvironmentState(const std::string& monitor_n
 
   tesseract_scene_graph::SceneState env_state;
   tesseract_rosutils::fromMsg(env_state.joints, res.response.joint_states);
+  tesseract_rosutils::fromMsg(env_state.floating_joints, res.response.floating_joint_states);
   tesseract_rosutils::fromMsg(env_state.link_transforms, res.response.link_transforms);
   tesseract_rosutils::fromMsg(env_state.joint_transforms, res.response.joint_transforms);
 
   return env_state;
 }
 
 bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& monitor_namespace,
-                                                         const std::unordered_map<std::string, double>& joints) const
+                                                         const std::unordered_map<std::string, double>& joints,
+                                                         const tesseract_common::TransformMap& floating_joints) const
 {
   tesseract_msgs::EnvironmentCommand command;
   command.command = tesseract_msgs::EnvironmentCommand::UPDATE_JOINT_STATE;
   tesseract_rosutils::toMsg(command.joint_state, joints);
+  tesseract_rosutils::toMsg(command.floating_joint_states, floating_joints);
   return sendCommands(env_name_, monitor_namespace, { command });
 }
 
 bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& monitor_namespace,
                                                          const std::vector<std::string>& joint_names,
-                                                         const std::vector<double>& joint_values) const
+                                                         const std::vector<double>& joint_values,
+                                                         const tesseract_common::TransformMap& floating_joints) const
 {
   std::unordered_map<std::string, double> joints;
   for (std::size_t i = 0; i < joint_names.size(); ++i)
@@ -271,12 +275,14 @@ bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& moni
   tesseract_msgs::EnvironmentCommand command;
   command.command = tesseract_msgs::EnvironmentCommand::UPDATE_JOINT_STATE;
   tesseract_rosutils::toMsg(command.joint_state, joints);
+  tesseract_rosutils::toMsg(command.floating_joint_states, floating_joints);
   return sendCommands(env_name_, monitor_namespace, { command });
 }
 
 bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& monitor_namespace,
                                                          const std::vector<std::string>& joint_names,
-                                                         const Eigen::Ref<const Eigen::VectorXd>& joint_values) const
+                                                         const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                                         const tesseract_common::TransformMap& floating_joints) const
 {
   std::unordered_map<std::string, double> joints;
   for (std::size_t i = 0; i < joint_names.size(); ++i)
@@ -285,42 +291,67 @@ bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& moni
   tesseract_msgs::EnvironmentCommand command;
   command.command = tesseract_msgs::EnvironmentCommand::UPDATE_JOINT_STATE;
   tesseract_rosutils::toMsg(command.joint_state, joints);
+  tesseract_rosutils::toMsg(command.floating_joint_states, floating_joints);
+  return sendCommands(env_name_, monitor_namespace, { command });
+}
+
+bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& monitor_namespace,
+                                                         const tesseract_common::TransformMap& floating_joints) const
+{
+  tesseract_msgs::EnvironmentCommand command;
+  command.command = tesseract_msgs::EnvironmentCommand::UPDATE_JOINT_STATE;
+  tesseract_rosutils::toMsg(command.floating_joint_states, floating_joints);
   return sendCommands(env_name_, monitor_namespace, { command });
 }
 
 std::vector<std::string>
-ROSEnvironmentMonitorInterface::setEnvironmentState(const std::unordered_map<std::string, double>& joints) const
+ROSEnvironmentMonitorInterface::setEnvironmentState(const std::unordered_map<std::string, double>& joints,
+                                                    const tesseract_common::TransformMap& floating_joints) const
 {
   std::vector<std::string> failed_namespace;
   failed_namespace.reserve(ns_.size());
   for (const auto& ns : ns_)
-    if (!setEnvironmentState(ns, joints))
+    if (!setEnvironmentState(ns, joints, floating_joints))
       failed_namespace.push_back(ns);
 
   return failed_namespace;
 }
 
 std::vector<std::string>
 ROSEnvironmentMonitorInterface::setEnvironmentState(const std::vector<std::string>& joint_names,
-                                                    const std::vector<double>& joint_values) const
+                                                    const std::vector<double>& joint_values,
+                                                    const tesseract_common::TransformMap& floating_joints) const
 {
   std::vector<std::string> failed_namespace;
   failed_namespace.reserve(ns_.size());
   for (const auto& ns : ns_)
-    if (!setEnvironmentState(ns, joint_names, joint_values))
+    if (!setEnvironmentState(ns, joint_names, joint_values, floating_joints))
       failed_namespace.push_back(ns);
 
   return failed_namespace;
 }
 
 std::vector<std::string>
 ROSEnvironmentMonitorInterface::setEnvironmentState(const std::vector<std::string>& joint_names,
-                                                    const Eigen::Ref<const Eigen::VectorXd>& joint_values) const
+                                                    const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                                    const tesseract_common::TransformMap& floating_joints) const
+{
+  std::vector<std::string> failed_namespace;
+  failed_namespace.reserve(ns_.size());
+  for (const auto& ns : ns_)
+    if (!setEnvironmentState(ns, joint_names, joint_values, floating_joints))
+      failed_namespace.push_back(ns);
+
+  return failed_namespace;
+}
+
+std::vector<std::string>
+ROSEnvironmentMonitorInterface::setEnvironmentState(const tesseract_common::TransformMap& floating_joints) const
 {
   std::vector<std::string> failed_namespace;
   failed_namespace.reserve(ns_.size());
   for (const auto& ns : ns_)
-    if (!setEnvironmentState(ns, joint_names, joint_values))
+    if (!setEnvironmentState(ns, floating_joints))
       failed_namespace.push_back(ns);
 
   return failed_namespace;

tesseract_msgs/msg/Environment.msg

@@ -1,2 +1,3 @@
 tesseract_msgs/EnvironmentCommand[] command_history
 sensor_msgs/JointState joint_states
+tesseract_msgs/TransformMap floating_joint_states

tesseract_msgs/msg/EnvironmentCommand.msg

@@ -72,6 +72,7 @@ string remove_allowed_collision_link
 
 # UPDATE_JOINT_STATE
 sensor_msgs/JointState joint_state
+tesseract_msgs/TransformMap floating_joint_states
 
 # ADD_SCENE_GRAPH Command
 tesseract_msgs/SceneGraph scene_graph

tesseract_msgs/msg/EnvironmentState.msg

@@ -3,3 +3,4 @@ uint64 revision # This is the revision number so others know when it changes.
 
 # This message contains information about the state, i.e. the positions of its joints and links
 sensor_msgs/JointState joint_state
+tesseract_msgs/TransformMap floating_joint_states

tesseract_msgs/srv/GetEnvironmentInformation.srv

@@ -39,3 +39,4 @@ tesseract_msgs/TransformMap joint_transforms
 tesseract_msgs/AllowedCollisionEntry[] allowed_collision_matrix
 tesseract_msgs/KinematicsInformation kinematics_information
 sensor_msgs/JointState joint_states
+tesseract_msgs/TransformMap floating_joint_states

tesseract_qt_ros/src/widgets/environment_monitor_widget.cpp

@@ -265,13 +265,15 @@ void EnvironmentMonitorWidget::snapshotCallback(const tesseract_msgs::Environmen
 
   tesseract_environment::Commands commands = tesseract_rosutils::fromMsg(msg->command_history);
   std::unordered_map<std::string, double> jv;
+  tesseract_common::TransformMap fjv;
   tesseract_rosutils::fromMsg(jv, msg->joint_states);
+  tesseract_rosutils::fromMsg(fjv, msg->floating_joint_states);
   auto env = std::make_shared<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
   env->setResourceLocator(locator);
   if (env->init(commands))
   {
-    env->setState(jv);
+    env->setState(jv, fjv);
 
     if (data_->monitor != nullptr)
       data_->monitor->shutdown();

tesseract_rosutils/include/tesseract_rosutils/utils.h

@@ -46,6 +46,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_collision/core/fwd.h>
 #include <tesseract_motion_planners/core/fwd.h>
 #include <tesseract_task_composer/core/fwd.h>
+#include <tesseract_msgs/TransformMap.h>
 
 #include <ros/message_forward.h>
 namespace tesseract_msgs
@@ -86,7 +87,6 @@ ROS_DECLARE_MESSAGE(StringPair)
 ROS_DECLARE_MESSAGE(Environment)
 ROS_DECLARE_MESSAGE(EnvironmentState)
 ROS_DECLARE_MESSAGE(Trajectory)
-ROS_DECLARE_MESSAGE(TransformMap)
 ROS_DECLARE_MESSAGE(VisualGeometry)
 ROS_DECLARE_MESSAGE(JointTrajectory)
 ROS_DECLARE_MESSAGE(PluginInfo)
@@ -135,6 +135,7 @@ class ROSResourceLocator : public tesseract_common::ResourceLocator
 };
 
 bool isMsgEmpty(const sensor_msgs::JointState& msg);
+bool isMsgEmpty(const tesseract_msgs::TransformMap& msg);
 
 bool isIdentical(const tesseract_geometry::Geometry& shape1, const tesseract_geometry::Geometry& shape2);
 
@@ -263,7 +264,9 @@ void toMsg(tesseract_msgs::JointTrajectory& traj_msg, const tesseract_common::Jo
  */
 tesseract_common::JointTrajectory fromMsg(const tesseract_msgs::JointTrajectory& traj_msg);
 
-bool processMsg(tesseract_environment::Environment& env, const sensor_msgs::JointState& joint_state_msg);
+bool processMsg(tesseract_environment::Environment& env,
+                const sensor_msgs::JointState& joint_state_msg,
+                const tesseract_msgs::TransformMap& floating_joint_state_msg = {});
 
 /**
  * @brief Apply the provided commands to the environment