Planning scene transcription

demo/src/pipeline_testbench_main.cpp

@@ -5,6 +5,7 @@
 #include <moveit/moveit_cpp/planning_component.h>
 
 #include <geometry_msgs/msg/point_stamped.h>
+#include <geometry_msgs/msg/pose.h>
 
 #include <moveit_visual_tools/moveit_visual_tools.h>
 
@@ -103,6 +104,13 @@ class Demo
     text_pose.translation().z() = 1.75;
     visual_tools_.publishText(text_pose, "Pipeline Testbench", rvt::WHITE, rvt::XLARGE);
     visual_tools_.trigger();
+
+    geometry_msgs::msg::Pose block_pose;
+    block_pose.position.z = 0.5;
+    block_pose.position.y = -0.3;
+    block_pose.position.x = 0.3;
+    visual_tools_.publishCollisionBlock(block_pose, "test_block", 0.15);
+
   }
 
   bool loadPlanningSceneAndQuery()
@@ -322,12 +330,8 @@ int main(int argc, char** argv)
   RCLCPP_INFO(LOGGER, "Starting Pipeline Testbench example ...");
   for (const auto& motion_plan_req : demo.getMotionPlanRequests())
   {
-    demo.planAndVisualize(
-        {
-        { "ompl", "RRTConnectkConfigDefault" },
-        { "stomp", "stomp" },
-        { "drake", ""} },
-        motion_plan_req);
+    demo.planAndVisualize({ { "ompl", "RRTConnectkConfigDefault" }, { "stomp", "stomp" }, { "drake", "" } },
+                          motion_plan_req);
   }
 
   demo.getVisualTools().prompt("Press 'next' in the RvizVisualToolsGui window to finish the demo");

include/ktopt_interface/ktopt_planning_context.hpp

@@ -12,63 +12,90 @@
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/planning/trajectory_optimization/kinematic_trajectory_optimization.h"
 #include "drake/solvers/solve.h"
+#include "drake/geometry/meshcat.h"
+#include "drake/geometry/meshcat_visualizer.h"
+#include "drake/geometry/drake_visualizer.h"
+#include "drake/geometry/meshcat_params.h"
+#include "drake/geometry/geometry_frame.h"
+#include "drake/geometry/geometry_instance.h"
+#include "drake/geometry/geometry_roles.h"
+#include "drake/geometry/proximity_properties.h"
+#include "drake/visualization/visualization_config.h"
+#include "drake/visualization/visualization_config_functions.h"
+#include <drake/multibody/inverse_kinematics/minimum_distance_lower_bound_constraint.h>
 
 namespace ktopt_interface
 {
 // declare all namespaces to be used
-using drake::multibody::MultibodyPlant;
-using drake::multibody::AddMultibodyPlantSceneGraph;
+using drake::geometry::Meshcat;
+using drake::geometry::MeshcatParams;
+using drake::geometry::MeshcatVisualizer;
+using drake::geometry::MeshcatVisualizerParams;
 using drake::geometry::SceneGraph;
-using drake::systems::DiagramBuilder;
+using drake::geometry::FrameId;
+using drake::geometry::SourceId;
+using drake::geometry::GeometryId;
+using drake::geometry::GeometryInstance;
+using drake::geometry::GeometryFrame;
+using drake::geometry::AddRigidHydroelasticProperties;
+using drake::geometry::IllustrationProperties;
+using drake::geometry::ProximityProperties;
+using drake::geometry::PerceptionProperties;
+using drake::geometry::Role;
+using drake::geometry::Box;
+using drake::multibody::AddMultibodyPlantSceneGraph;
+using drake::multibody::MultibodyPlant;
+using drake::multibody::PackageMap;
 using drake::multibody::Parser;
+using drake::multibody::MultibodyPlant;
+using drake::multibody::MinimumDistanceLowerBoundConstraint;
 using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization;
 using drake::solvers::Solve;
-using drake::systems::Diagram;
 using drake::systems::Context;
-using drake::multibody::PackageMap;
-using Eigen::VectorXd;
+using drake::systems::Diagram;
+using drake::systems::DiagramBuilder;
+using drake::visualization::ApplyVisualizationConfig;
+using drake::visualization::VisualizationConfig;
+using drake::math::RigidTransformd;
 using Eigen::MatrixXd;
+using Eigen::VectorXd;
+using Eigen::Vector3d;
 using Joints = std::vector<const moveit::core::JointModel*>;
 
 class KTOptPlanningContext : public planning_interface::PlanningContext
 {
 public:
-    KTOptPlanningContext(
-        const std::string& name,
-        const std::string& group_name,
-        const ktopt_interface::Params& params);
-
-    void solve(
-        planning_interface::MotionPlanResponse& res) override;
-    void solve(
-        planning_interface::MotionPlanDetailedResponse& res) override;
+  KTOptPlanningContext(const std::string& name, const std::string& group_name, const ktopt_interface::Params& params);
 
-    bool terminate() override;
-    void clear() override;
+  void solve(planning_interface::MotionPlanResponse& res) override;
+  void solve(planning_interface::MotionPlanDetailedResponse& res) override;
 
-    void setRobotDescription(std::string robot_description);
-    VectorXd toDrakePositions(
-        const moveit::core::RobotState& state,
-        const Joints& joints);
-    void setJointPositions(
-        const VectorXd& values,
-        const Joints& joints,
-        moveit::core::RobotState& state);
-    void setJointVelocities(
-        const VectorXd& values,
-        const Joints& joints,
-        moveit::core::RobotState& state);
+  bool terminate() override;
+  void clear() override;
 
+  void setRobotDescription(std::string robot_description);
+  VectorXd toDrakePositions(const moveit::core::RobotState& state, const Joints& joints);
+  void setJointPositions(const VectorXd& values, const Joints& joints, moveit::core::RobotState& state);
+  void setJointVelocities(const VectorXd& values, const Joints& joints, moveit::core::RobotState& state);
+  void transcribePlanningScene(const planning_scene::PlanningScene& planning_scene);
 
 private:
-    const ktopt_interface::Params params_;
-    std::string robot_description_;
+  const ktopt_interface::Params params_;
+  std::string robot_description_;
+
+  // drake related variables
+  // SceneGraph<double>* scene_graph_{};
+  // MultibodyPlant<double>* plant_{};
+  std::unique_ptr<Diagram<double>> diagram_;
+  std::unique_ptr<DiagramBuilder<double>> builder;
+  std::unique_ptr<Context<double>> diagram_context_;
+  // Context<double>* plant_context_{};
+  // Context<double>* visualizer_context_{};
+  VectorXd nominal_q_;
+  std::string OCTOMAP_NS = "<octomap>";
 
-    // drake related variables
-    SceneGraph<double>* scene_graph_{};
-    MultibodyPlant<double>* plant_{};
-    std::unique_ptr<Context<double>> diagram_context_;
-    Context<double>* plant_context_;
-    VectorXd nominal_q_;
+  // visualization
+  std::shared_ptr<Meshcat> meshcat_;
+  MeshcatVisualizer<double>* visualizer_;
 };
-} // namespace ktopt_interface
+}  // namespace ktopt_interface

src/ktopt_planner_manager.cpp

@@ -31,24 +31,24 @@ class KTOptPlannerManager : public planning_interface::PlannerManager
     node_ = node;
     param_listener_ = std::make_shared<ktopt_interface::ParamListener>(node, parameter_namespace);
 
-
-  // set QoS to transient local to get messages that have already been published
-  // (if robot state publisher starts before planner)
+    // set QoS to transient local to get messages that have already been published
+    // (if robot state publisher starts before planner)
     robot_description_subscriber_ = node_->create_subscription<std_msgs::msg::String>(
-      "robot_description", rclcpp::QoS(1).transient_local(), [this](const std_msgs::msg::String::SharedPtr msg){
-        if (robot_description_.empty())
-        {
-          robot_description_ = msg->data;
-          RCLCPP_INFO(getLogger(), "Robot description set");
-        }
-      });
+        "robot_description", rclcpp::QoS(1).transient_local(), [this](const std_msgs::msg::String::SharedPtr msg) {
+          if (robot_description_.empty())
+          {
+            robot_description_ = msg->data;
+            RCLCPP_INFO(getLogger(), "Robot description set");
+          }
+        });
     RCLCPP_INFO(getLogger(), "KTOpt planner manager initialized!");
     return true;
   }
 
   bool canServiceRequest(const planning_interface::MotionPlanRequest& req) const override
   {
-    if(robot_description_.empty()){
+    if (robot_description_.empty())
+    {
       RCLCPP_ERROR(getLogger(), "Robot description is empty, do you have a robot state publisher running?");
       return false;
     }
@@ -93,8 +93,8 @@ class KTOptPlannerManager : public planning_interface::PlannerManager
     std::shared_ptr<KTOptPlanningContext> planning_context =
         std::make_shared<KTOptPlanningContext>("KTOPT", req.group_name, params);
     // set robot description
-    planning_context->setRobotDescription(robot_description_);
     planning_context->setPlanningScene(planning_scene);
+    planning_context->setRobotDescription(robot_description_);
     planning_context->setMotionPlanRequest(req);
 
     return planning_context;