Environment should be stored as const in data storage

tesseract-robotics · Jul 18, 2024 · 44fb098 · 44fb098
1 parent 2122689
commit 44fb098
Show file tree

Hide file tree

Showing 24 changed files with 85 additions and 96 deletions.
diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp
@@ -271,7 +271,7 @@ bool BasicCartesianExample::run()
   // Create Task Composer Data Storage
   auto data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
   data->setData("planning_input", program);
-  data->setData("environment", env_);
+  data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
   data->setData("profiles", profiles);
 
   if (plotter_ != nullptr && plotter_->isConnected())

diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp
@@ -378,7 +378,7 @@ bool CarSeatExample::run()
     // Create Task Composer Data Storage
     auto data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
     data->setData("planning_input", program);
-    data->setData("environment", env_);
+    data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
     data->setData("profiles", profiles);
 
     // Solve task
@@ -465,7 +465,7 @@ bool CarSeatExample::run()
     // Create Task Composer Data Storage
     auto data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
     data->setData("planning_input", program);
-    data->setData("environment", env_);
+    data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
     data->setData("profiles", profiles);
 
     // Solve task

diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp
@@ -229,7 +229,7 @@ bool FreespaceHybridExample::run()
   // Create Task Composer Data Storage
   auto data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
   data->setData("planning_input", program);
-  data->setData("environment", env_);
+  data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
   data->setData("profiles", profiles);
 
   // Solve task

diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp
@@ -197,7 +197,7 @@ bool FreespaceOMPLExample::run()
   // Create Task Composer Data Storage
   auto data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
   data->setData("planning_input", program);
-  data->setData("environment", env_);
+  data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
   data->setData("profiles", profiles);
 
   // Solve task

diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp
@@ -261,7 +261,7 @@ bool GlassUprightExample::run()
   // Create Task Composer Data Storage
   auto data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
   data->setData("planning_input", program);
-  data->setData("environment", env_);
+  data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
   data->setData("profiles", profiles);
 
   if (plotter_ != nullptr && plotter_->isConnected())

diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp
@@ -306,7 +306,7 @@ bool PickAndPlaceExample::run()
   // Create Task Composer Data Storage
   auto pick_data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
   pick_data->setData("planning_input", pick_program);
-  pick_data->setData("environment", env_);
+  pick_data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
   pick_data->setData("profiles", profiles);
 
   // Solve task
@@ -425,7 +425,7 @@ bool PickAndPlaceExample::run()
   // Create Task Composer Data Storage
   auto place_data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
   place_data->setData("planning_input", place_program);
-  place_data->setData("environment", env_);
+  place_data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
   place_data->setData("profiles", profiles);
 
   // Solve task

diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp
@@ -307,7 +307,7 @@ bool PuzzlePieceAuxillaryAxesExample::run()
   // Create Task Composer Data Storage
   auto data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
   data->setData("planning_input", program);
-  data->setData("environment", env_);
+  data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
   data->setData("profiles", profiles);
 
   // Solve task

diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp
@@ -293,7 +293,7 @@ bool PuzzlePieceExample::run()
   // Create Task Composer Data Storage
   auto data = std::make_unique<tesseract_planning::TaskComposerDataStorage>();
   data->setData("planning_input", program);
-  data->setData("environment", env_);
+  data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env_));
   data->setData("profiles", profiles);
 
   // Solve task

diff --git a/tesseract_task_composer/examples/task_composer_raster_example.cpp b/tesseract_task_composer/examples/task_composer_raster_example.cpp
@@ -67,7 +67,7 @@ int main()
   // Create data storage
   auto task_data = std::make_unique<TaskComposerDataStorage>();
   task_data->setData(input_key, program);
-  task_data->setData("environment", env);
+  task_data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env));
   task_data->setData("profiles", profiles);
 
   // Solve raster plan

diff --git a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp
@@ -68,7 +68,7 @@ int main()
   // Create data storage
   auto task_data = std::make_unique<TaskComposerDataStorage>();
   task_data->setData(input_key, program);
-  task_data->setData("environment", env);
+  task_data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(env));
   task_data->setData("profiles", profiles);
 
   auto task_executor = factory.createTaskComposerExecutor("TaskflowExecutor");

diff --git a/..._composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/..._composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp
@@ -149,17 +149,16 @@ class MotionPlannerTask : public TaskComposerTask
     // Check that inputs are valid
     // --------------------
     auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-    if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+    if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
     {
       info->status_code = 0;
-      info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is missing";
+      info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
       CONSOLE_BRIDGE_logError("%s", info->status_message.c_str());
       info->return_value = 0;
       return info;
     }
 
-    std::shared_ptr<const tesseract_environment::Environment> env =
-        env_poly.template as<std::shared_ptr<tesseract_environment::Environment>>();
+    auto env = env_poly.template as<std::shared_ptr<const tesseract_environment::Environment>>();
     info->env = env;
 
     auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);

diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp
@@ -113,7 +113,7 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -122,8 +122,7 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
   info->env = env;
 
   auto input_data_poly = getData(*context.data_storage, INPUT_PROGRAM_PORT);

diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp
@@ -113,7 +113,7 @@ std::unique_ptr<TaskComposerNodeInfo> DiscreteContactCheckTask::runImpl(TaskComp
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -122,8 +122,7 @@ std::unique_ptr<TaskComposerNodeInfo> DiscreteContactCheckTask::runImpl(TaskComp
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
   info->env = env;
 
   auto input_data_poly = getData(*context.data_storage, INPUT_PROGRAM_PORT);

diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp
@@ -106,16 +106,15 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateBoundsTask::runImpl(TaskComposerCo
   // Check that inputs are valid
   // --------------------
   tesseract_common::AnyPoly env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
     CONSOLE_BRIDGE_logError("%s", info->status_message.c_str());
     info->return_value = 0;
     return info;
   }
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
 
   auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);
   if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))

diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp
@@ -399,7 +399,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateCollisionTask::runImpl(TaskCompose
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -408,8 +408,7 @@ std::unique_ptr<TaskComposerNodeInfo> FixStateCollisionTask::runImpl(TaskCompose
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
   info->env = env;
 
   auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);

diff --git a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp
@@ -128,7 +128,7 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context,
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -137,8 +137,7 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context,
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
 
   auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);
   if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))

diff --git a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp
@@ -108,7 +108,7 @@ std::unique_ptr<TaskComposerNodeInfo> MinLengthTask::runImpl(TaskComposerContext
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -117,8 +117,7 @@ std::unique_ptr<TaskComposerNodeInfo> MinLengthTask::runImpl(TaskComposerContext
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
 
   auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);
   if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))

diff --git a/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp b/tesseract_task_composer/planning/src/nodes/raster_motion_task.cpp
@@ -360,7 +360,7 @@ std::unique_ptr<TaskComposerNodeInfo> RasterMotionTask::runImpl(TaskComposerCont
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -369,8 +369,7 @@ std::unique_ptr<TaskComposerNodeInfo> RasterMotionTask::runImpl(TaskComposerCont
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
   info->env = env;
 
   auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);

diff --git a/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp b/tesseract_task_composer/planning/src/nodes/raster_only_motion_task.cpp
@@ -301,7 +301,7 @@ std::unique_ptr<TaskComposerNodeInfo> RasterOnlyMotionTask::runImpl(TaskComposer
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -310,8 +310,7 @@ std::unique_ptr<TaskComposerNodeInfo> RasterOnlyMotionTask::runImpl(TaskComposer
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
   info->env = env;
 
   tesseract_common::ManipulatorInfo input_manip_info;

diff --git a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp
@@ -112,7 +112,7 @@ RuckigTrajectorySmoothingTask::runImpl(TaskComposerContext& context, OptionalTas
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -121,8 +121,7 @@ RuckigTrajectorySmoothingTask::runImpl(TaskComposerContext& context, OptionalTas
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
 
   auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);
   if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))

diff --git a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp
@@ -119,7 +119,7 @@ TimeOptimalParameterizationTask::runImpl(TaskComposerContext& context, OptionalT
   // Check that inputs are valid
   // --------------------
   auto env_poly = getData(*context.data_storage, INPUT_ENVIRONMENT_PORT);
-  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<tesseract_environment::Environment>)))
+  if (env_poly.getType() != std::type_index(typeid(std::shared_ptr<const tesseract_environment::Environment>)))
   {
     info->status_code = 0;
     info->status_message = "Input data '" + input_keys_.get(INPUT_ENVIRONMENT_PORT) + "' is not correct type";
@@ -128,8 +128,7 @@ TimeOptimalParameterizationTask::runImpl(TaskComposerContext& context, OptionalT
     return info;
   }
 
-  std::shared_ptr<const tesseract_environment::Environment> env =
-      env_poly.as<std::shared_ptr<tesseract_environment::Environment>>();
+  auto env = env_poly.as<std::shared_ptr<const tesseract_environment::Environment>>();
 
   auto input_data_poly = getData(*context.data_storage, INOUT_PROGRAM_PORT);
   if (input_data_poly.getType() != std::type_index(typeid(CompositeInstruction)))

diff --git a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp
@@ -86,7 +86,7 @@ void checkProgram(const Environment::Ptr& env,
   // Create data storage
   auto task_data = std::make_unique<TaskComposerDataStorage>();
   task_data->setData("input_program", program);
-  task_data->setData("environment", env);
+  task_data->setData("environment", std::shared_ptr<const Environment>(env));
   task_data->setData("profiles", profiles);
 
   // Create context

diff --git a/tesseract_task_composer/test/fix_state_collision_task_unit.cpp b/tesseract_task_composer/test/fix_state_collision_task_unit.cpp
@@ -49,7 +49,7 @@ TEST_F(FixStateCollisionTaskUnit, StateInCollisionTest)  // NOLINT
   // Create data storage
   auto task_data = std::make_unique<TaskComposerDataStorage>();
   task_data->setData("input_program", program);
-  task_data->setData("environment", env_);
+  task_data->setData("environment", std::shared_ptr<const Environment>(env_));
 
   FixStateCollisionProfile profile;
 
@@ -90,7 +90,7 @@ TEST_F(FixStateCollisionTaskUnit, WaypointInCollisionTest)  // NOLINT
   // Create data storage
   auto task_data = std::make_unique<TaskComposerDataStorage>();
   task_data->setData("input_program", program);
-  task_data->setData("environment", env_);
+  task_data->setData("environment", std::shared_ptr<const Environment>(env_));
 
   FixStateCollisionProfile profile;
 
@@ -139,7 +139,7 @@ TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionRandomSamplerTest)  /
   // Create data storage
   auto task_data = std::make_shared<TaskComposerDataStorage>();
   task_data->setData("input_program", program);
-  task_data->setData("environment", env_);
+  task_data->setData("environment", std::shared_ptr<const Environment>(env_));
 
   FixStateCollisionProfile profile;
 
@@ -173,7 +173,7 @@ TEST_F(FixStateCollisionTaskUnit, MoveWaypointFromCollisionTrajoptTest)  // NOLI
   // Create data storage
   auto task_data = std::make_shared<TaskComposerDataStorage>();
   task_data->setData("input_program", program);
-  task_data->setData("environment", env_);
+  task_data->setData("environment", std::shared_ptr<const Environment>(env_));
 
   FixStateCollisionProfile profile;