Update 0.14.0

twc_application/config/twc2.rviz

@@ -0,0 +1,152 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TesseractWorkbench1/Environment Properties1
+      Splitter Ratio: 0.5
+    Tree Height: 815
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: tesseract_rviz/TesseractWorkbench
+      Enabled: true
+      Environment Properties:
+        Display Mode: Monitor
+        Joint State Topic: /joint_states
+        Monitor Topic: /tesseract_workcell_environment/tesseract_published_environment
+        URDF Parameter: robot_description
+        Value: ""
+      Joint Trajectory Properties:
+        Legacy Joint Trajectory:
+          Value: true
+          topic: /joint_trajectory
+        Tesseract Joint Trajectory:
+          Joint Trajectory Topic: /tesseract_trajectory
+          Value: true
+        Value: ""
+      Name: TesseractWorkbench
+      Value: true
+      tesseract::EnvMonitorJointStateTopic: /joint_states
+      tesseract::EnvMonitorMode: Monitor
+      tesseract::EnvMonitorTopic: /tesseract_workcell_environment/tesseract_published_environment
+      tesseract::EnvMonitorURDFDescription: robot_description
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /tesseract/display_tool_path
+      Name: ToolPath
+      Namespaces:
+        tesseract/segment_0/poses: true
+      Queue Size: 100
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 6.527585983276367
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -0.34869617223739624
+        Y: -0.00801808200776577
+        Z: 0.7818729877471924
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4103984832763672
+      Target Frame: <Fixed Frame>
+      Yaw: 0.24539875984191895
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1136
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000240000003d7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc0000003d000003d7000003520100001cfa000000010100000002fb000000240054006500730073006500720061006300740057006f0072006b00620065006e006300680100000000ffffffff0000024000fffffffb000000100044006900730070006c0061007900730100000000000002400000015600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003d7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003d7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000039fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000003dd000003d700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  TesseractWorkbench:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

twc_application/include/twc_application/application.h

@@ -5,7 +5,7 @@
 #include <tesseract_visualization/visualization.h>
 #include <tesseract_visualization/markers/toolpath_marker.h>
 #include <tesseract_motion_planners/core/utils.h>
-#include <tesseract_command_language/core/instruction.h>
+#include <tesseract_command_language/poly/instruction_poly.h>
 #include <tesseract_command_language/composite_instruction.h>
 #include <tesseract_common/types.h>
 
@@ -25,7 +25,7 @@ class Application
 
   virtual void run() = 0;
 
-  virtual void plotToolpath(const tesseract_planning::Instruction& program)
+  virtual void plotToolpath(const tesseract_planning::InstructionPoly& program)
   {
     if (plotter_ != nullptr && env_ != nullptr)
     {
@@ -35,19 +35,16 @@ class Application
     }
   }
 
-  virtual void plotTrajectory(const tesseract_planning::Instruction& program)
+  virtual void plotTrajectory(const tesseract_planning::InstructionPoly& program)
   {
     if (plotter_ != nullptr && env_ != nullptr)
     {
-      const auto& ci = program.as<tesseract_planning::CompositeInstruction>();
-      long num_wp = tesseract_planning::getMoveInstructionCount(ci);
-
       tesseract_common::Toolpath tp = tesseract_planning::toToolpath(program, *env_);
       plotter_->plotMarker(tesseract_visualization::ToolpathMarker(tp));
       plotter_->waitForInput("Preview Trajectory Toolpath. Hit enter to continue!");
 
       auto state_solver = env_->getStateSolver();
-      plotter_->plotTrajectory(tesseract_planning::toJointTrajectory(ci), *state_solver);
+      plotter_->plotTrajectory(tesseract_planning::toJointTrajectory(program), *state_solver);
       plotter_->waitForInput("Preview Trajectory. Hit enter to continue!");
     }
   }

twc_application/include/twc_application/raster_applicataion.h

@@ -12,8 +12,12 @@
 #include <tesseract_msgs/GetMotionPlanAction.h>
 #include <tesseract_rosutils/plotting.h>
 #include <tesseract_rosutils/conversions.h>
-#include <tesseract_command_language/core/serialization.h>
-#include <tesseract_command_language/utils/utils.h>
+#include <tesseract_common/serialization.h>
+#include <tesseract_command_language/utils.h>
+#include <tesseract_command_language/move_instruction.h>
+#include <tesseract_command_language/cartesian_waypoint.h>
+#include <tesseract_command_language/state_waypoint.h>
+#include <tesseract_command_language/joint_waypoint.h>
 #include <tesseract_motion_planners/core/utils.h>
 #include <tesseract_visualization/visualization_loader.h>
 #include <tesseract_visualization/markers/toolpath_marker.h>
@@ -68,9 +72,9 @@ class RasterApplication : public Application
 
     // Create motion planning goal request
     tesseract_msgs::GetMotionPlanGoal goal;
-    goal.request.name = goal.request.RASTER_G_FT_PLANNER_NAME;
+    goal.request.name = "RasterFtPipeline";
 //    goal.request.name = "RasterGDebug";
-    goal.request.instructions = tesseract_planning::Serialization::toArchiveStringXML<tesseract_planning::Instruction>(program);
+    goal.request.instructions = tesseract_common::Serialization::toArchiveStringXML<tesseract_planning::InstructionPoly>(program);
 
     // Send goal
     ac_.sendGoal(goal);
@@ -80,7 +84,7 @@ class RasterApplication : public Application
     ROS_INFO("Action finished: %s", state.toString().c_str());
 
     auto result = ac_.getResult();
-    tesseract_planning::Instruction program_results = tesseract_planning::Serialization::fromArchiveStringXML<tesseract_planning::Instruction>(result->response.results);
+    tesseract_planning::InstructionPoly program_results = tesseract_common::Serialization::fromArchiveStringXML<tesseract_planning::InstructionPoly>(result->response.results);
 
     if (!result->response.successful)
     {
@@ -93,7 +97,7 @@ class RasterApplication : public Application
     }
   }
 
-  tesseract_planning::CompositeInstruction createProgram(const tesseract_planning::ManipulatorInfo& manip_info, const tesseract_common::Toolpath& raster_strips)
+  tesseract_planning::CompositeInstruction createProgram(const tesseract_common::ManipulatorInfo& manip_info, const tesseract_common::Toolpath& raster_strips)
   {
 
     std::string raster_profile {"RASTER_ROBOT"};
@@ -117,21 +121,22 @@ class RasterApplication : public Application
 
     tesseract_planning::CompositeInstruction program("raster_program", tesseract_planning::CompositeInstructionOrder::ORDERED, manip_info);
 
-    tesseract_planning::StateWaypoint swp1(joint_names, Eigen::VectorXd::Zero(joint_names.size()));
-    tesseract_planning::PlanInstruction start_instruction(swp1, tesseract_planning::PlanInstructionType::START, freespace_profile);
-    program.setStartInstruction(start_instruction);
+    tesseract_planning::StateWaypointPoly swp1 = tesseract_planning::StateWaypoint(joint_names, Eigen::VectorXd::Zero(joint_names.size()));
+    tesseract_planning::MoveInstruction start_instruction(swp1, tesseract_planning::MoveInstructionType::FREESPACE, freespace_profile);
+    start_instruction.setDescription("Start Instruction");
 
     for (std::size_t rs = 0; rs < raster_strips.size(); ++rs)
     {
       if (rs == 0)
       {
         // Define from start composite instruction
-        tesseract_planning::CartesianWaypoint wp1 = raster_strips[rs][0];
-        tesseract_planning::PlanInstruction plan_f0(wp1, tesseract_planning::PlanInstructionType::FREESPACE, freespace_profile);
+        tesseract_planning::CartesianWaypointPoly wp1 = tesseract_planning::CartesianWaypoint(raster_strips[rs][0]);
+        tesseract_planning::MoveInstruction plan_f0(wp1, tesseract_planning::MoveInstructionType::FREESPACE, freespace_profile);
         plan_f0.setDescription("from_start_plan");
         tesseract_planning::CompositeInstruction from_start(freespace_profile);
         from_start.setDescription("from_start");
-        from_start.push_back(plan_f0);
+        from_start.appendMoveInstruction(start_instruction);
+        from_start.appendMoveInstruction(plan_f0);
         program.push_back(from_start);
       }
 
@@ -141,34 +146,33 @@ class RasterApplication : public Application
 
       for (std::size_t i = 1; i < raster_strips[rs].size(); ++i)
       {
-        tesseract_planning::CartesianWaypoint wp = raster_strips[rs][i];
-        raster_segment.push_back(tesseract_planning::PlanInstruction(wp, tesseract_planning::PlanInstructionType::LINEAR, raster_profile));
+        tesseract_planning::CartesianWaypointPoly wp = tesseract_planning::CartesianWaypoint(raster_strips[rs][i]);
+        raster_segment.appendMoveInstruction(tesseract_planning::MoveInstruction(wp, tesseract_planning::MoveInstructionType::LINEAR, raster_profile));
       }
       program.push_back(raster_segment);
 
 
       if (rs < raster_strips.size() - 1)
       {
         // Add transition
-        tesseract_planning::CartesianWaypoint twp = raster_strips[rs + 1].front();
-
-        tesseract_planning::PlanInstruction tranisiton_instruction1(twp, tesseract_planning::PlanInstructionType::FREESPACE, transition_profile);
+        tesseract_planning::CartesianWaypointPoly twp = tesseract_planning::CartesianWaypoint(raster_strips[rs + 1].front());
+        tesseract_planning::MoveInstruction tranisiton_instruction1(twp, tesseract_planning::MoveInstructionType::FREESPACE, transition_profile);
         tranisiton_instruction1.setDescription("transition_from_end_plan");
 
         tesseract_planning::CompositeInstruction transition(transition_profile);
         transition.setDescription("transition_from_end");
-        transition.push_back(tranisiton_instruction1);
+        transition.appendMoveInstruction(tranisiton_instruction1);
 
         program.push_back(transition);
       }
       else
       {
         // Add to end instruction
-        tesseract_planning::PlanInstruction plan_f2(swp1, tesseract_planning::PlanInstructionType::FREESPACE, freespace_profile);
+        tesseract_planning::MoveInstruction plan_f2(swp1, tesseract_planning::MoveInstructionType::FREESPACE, freespace_profile);
         plan_f2.setDescription("to_end_plan");
         tesseract_planning::CompositeInstruction to_end(freespace_profile);
         to_end.setDescription("to_end");
-        to_end.push_back(plan_f2);
+        to_end.appendMoveInstruction(plan_f2);
         program.push_back(to_end);
       }
     }

twc_application/launch/application_base.launch

@@ -7,6 +7,11 @@
   <arg name="continuous_plugin" default="BulletCastBVHManager"/>
   <arg name="joint_state_topic" default=""/>
   <arg name="monitor_namespace" default="tesseract_workcell_environment"/>
+  <arg name="cache_size" default="5"/>
+  <arg name="cache_refresh_rate" default="0.1"/>
+  <arg name="task_composer_config" default="$(find tesseract_task_composer)/config/task_composer_plugins.yaml"/>
+  <arg name="input_key" default="input_data"/>
+  <arg name="output_key" default="output_data"/>
 
   <!-- Launch Tesseract Monitor -->
   <node pkg="tesseract_monitoring" type="tesseract_monitoring_environment_node" name="tesseract_environment_monitor" output="screen" required="true">
@@ -26,6 +31,11 @@
     <param name="continuous_plugin" type="string" value="$(arg continuous_plugin)"/>
     <param name="monitor_namespace" type="string" value="$(arg monitor_namespace)_planning_server"/>
     <param name="monitored_namespace" type="string" value="$(arg monitor_namespace)"/>
+    <param name="cache_size" type="int" value="$(arg cache_size)"/>
+    <param name="cache_refresh_rate" type="double" value="$(arg cache_refresh_rate)"/>
+    <param name="task_composer_config" type="string" value="$(arg task_composer_config)"/>
+    <param name="input_key" type="string" value="$(arg input_key)"/>
+    <param name="output_key" type="string" value="$(arg output_key)"/>
   </node>
 
   <!-- Launch Tesseract Contact Monitor -->

twc_application/launch/application_positioner.launch

@@ -27,8 +27,8 @@
   </include>
 
   <!-- Launch RViz -->
-  <node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz" args="-d $(find twc_application)/config/twc.rviz" output="screen" required="true"/>
-
+<!--  <node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz" args="-d $(find twc_application)/config/twc.rviz" output="screen" required="true"/>-->
+<node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz" output="screen" required="true"/>
   <!-- Launch GUI to drive joints   -->
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
 

twc_motion_planning/CMakeLists.txt

@@ -14,7 +14,8 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(Boost REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(tesseract_command_language REQUIRED)
-find_package(tesseract_process_managers REQUIRED)
+find_package(tesseract_task_composer REQUIRED)
+find_package(tesseract_motion_planners REQUIRED)
 find_package(tesseract_common REQUIRED)
 find_package(tesseract_collision REQUIRED)
 
@@ -25,8 +26,8 @@ catkin_package(
     tesseract_planning_server
   DEPENDS
     tesseract_command_language
-    tesseract_process_managers
-    tesseract_planning_server
+    tesseract_task_composer
+    tesseract_motion_planners
 )
 
 # Load variable for clang tidy args, compiler options and cxx version
@@ -36,7 +37,12 @@ add_executable(${PROJECT_NAME}_planning_server_node src/twc_planning_server_node
 target_link_libraries(${PROJECT_NAME}_planning_server_node PRIVATE
   tesseract::tesseract_collision_core
   tesseract::tesseract_command_language
-  tesseract::tesseract_process_managers
+  tesseract::tesseract_task_composer
+  tesseract::tesseract_motion_planners_core
+  tesseract::tesseract_motion_planners_trajopt
+  tesseract::tesseract_motion_planners_ompl
+  tesseract::tesseract_motion_planners_descartes
+  tesseract::tesseract_motion_planners_simple
   ${catkin_LIBRARIES})
 target_compile_options(${PROJECT_NAME}_planning_server_node PRIVATE ${TESSERACT_COMPILE_OPTIONS})
 target_clang_tidy(${PROJECT_NAME}_planning_server_node ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})

twc_motion_planning/include/twc_motion_planning/utils.h

@@ -60,7 +60,7 @@ class DescartesStateValidator : public tesseract_planning::DescartesVertexEvalua
     if (start_redun != Eigen::Vector3i::Zero())
       return false;
 
-    return tesseract_common::satisfiesPositionLimits(vertex, limits_);
+    return tesseract_common::satisfiesPositionLimits<double>(vertex, limits_);
   }
 
 protected: