Merge pull request RobotLocomotion#6199 from sammy-tri/world_state_dedup

Deduplicate the world state implementations

drake/examples/kuka_iiwa_arm/CMakeLists.txt

@@ -101,8 +101,9 @@ if(lcm_FOUND)
 endif()
 
 add_subdirectory(controlled_kuka)
-add_subdirectory(iiwa_world)
 add_subdirectory(dev)
+add_subdirectory(iiwa_world)
+add_subdirectory(pick_and_place)
 
 if(BUILD_TESTING)
   add_subdirectory(test)

drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/BUILD

@@ -17,16 +17,15 @@ drake_cc_library(
     srcs = [
         "demo_diagram_builder.cc",
         "state_machine_system.cc",
-        "synchronous_world_state.cc",
     ],
     hdrs = [
         "demo_diagram_builder.h",
         "pick_and_place_common.h",
         "state_machine_system.h",
-        "synchronous_world_state.h",
     ],
     deps = [
         "//drake/examples/kuka_iiwa_arm:iiwa_common",
+        "//drake/examples/kuka_iiwa_arm/pick_and_place",
         "//drake/examples/kuka_iiwa_arm:robot_plan_interpolator",
         "//drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/action_primitives",
         "//drake/examples/kuka_iiwa_arm/dev/pick_and_place:kuka_pick_and_place",

drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/state_machine_system.cc

@@ -8,7 +8,6 @@
 #include "drake/common/drake_path.h"
 #include "drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/action_primitives/action_primitives_common.h"
 #include "drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/pick_and_place_common.h"
-#include "drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/synchronous_world_state.h"
 #include "drake/examples/kuka_iiwa_arm/dev/pick_and_place/pick_and_place_common.h"
 
 using bot_core::robot_state_t;
@@ -74,7 +73,8 @@ PickAndPlaceStateMachineSystem::PickAndPlaceStateMachineSystem(
       planner_(std::make_unique<ConstraintRelaxingIk>(
           drake::GetDrakePath() + kIiwaUrdf, kIiwaEndEffectorName, iiwa_base_)),
       world_state_(
-          std::make_unique<SynchronousWorldState>(planner_->get_robot())) {
+          std::make_unique<pick_and_place::WorldState>(
+              drake::GetDrakePath() + kIiwaUrdf, kIiwaEndEffectorName)) {
   input_port_iiwa_state_ = this->DeclareAbstractInputPort().get_index();
   input_port_box_state_ = this->DeclareAbstractInputPort().get_index();
   input_port_wsg_status_ = this->DeclareAbstractInputPort().get_index();
@@ -173,9 +173,9 @@ void PickAndPlaceStateMachineSystem::DoCalcUnrestrictedUpdate(
       this->EvalAbstractInput(context, input_port_wsg_action_status_)
           ->GetValue<ActionPrimitiveState>();
 
-  world_state_->UnpackIiwaStatusMessage(&iiwa_state);
-  world_state_->UnpackWsgStatusMessage(&wsg_status);
-  world_state_->UnpackObjectStatusMessage(&box_state);
+  world_state_->HandleIiwaStatus(iiwa_state);
+  world_state_->HandleWsgStatus(wsg_status);
+  world_state_->HandleObjectStatus(box_state);
 
   IKResults ik_res;
   std::vector<double> times;

drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/state_machine_system.h

@@ -5,7 +5,7 @@
 
 #include "bot_core/robot_state_t.hpp"
 
-#include "drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/synchronous_world_state.h"
+#include "drake/examples/kuka_iiwa_arm/pick_and_place/world_state.h"
 #include "drake/manipulation/planner/constraint_relaxing_ik.h"
 #include "drake/multibody/rigid_body_tree.h"
 #include "drake/systems/framework/leaf_system.h"
@@ -150,7 +150,7 @@ class PickAndPlaceStateMachineSystem : public systems::LeafSystem<double> {
 
   const std::unique_ptr<
     manipulation::planner::ConstraintRelaxingIk> planner_{nullptr};
-  const std::unique_ptr<SynchronousWorldState> world_state_{nullptr};
+  const std::unique_ptr<pick_and_place::WorldState> world_state_{nullptr};
 };
 
 }  // namespace monolithic_pick_and_place

drake/examples/kuka_iiwa_arm/dev/pick_and_place/BUILD

@@ -17,15 +17,14 @@ drake_cc_library(
     srcs = [
         "action.cc",
         "pick_and_place_common.cc",
-        "world_state.cc",
     ],
     hdrs = [
         "action.h",
         "pick_and_place_common.h",
-        "world_state.h",
     ],
     deps = [
         "//drake/common/trajectories:piecewise_quaternion",
+        "//drake/examples/kuka_iiwa_arm/pick_and_place",
         "//drake/examples/kuka_iiwa_arm:iiwa_common",
         "//drake/lcmtypes:schunk",
         "//drake/manipulation/planner:constraint_relaxing_ik",

drake/examples/kuka_iiwa_arm/dev/pick_and_place/CMakeLists.txt

@@ -1,28 +1,25 @@
 if(lcm_FOUND)
-  add_library_with_exports(LIB_NAME pickAndPlaceCommon SOURCE_FILES
+  add_library_with_exports(LIB_NAME pickAndPlaceDevCommon SOURCE_FILES
       action.cc
-      pick_and_place_common.cc
-      world_state.cc)
-  target_link_libraries(pickAndPlaceCommon
+      pick_and_place_common.cc)
+  target_link_libraries(pickAndPlaceDevCommon
       drakeCommon
       drakeIK
       drakeKukaIiwaArmCommon
       drakeManipulationPlanner
-      drakeLcmSystem
       drakeLCMTypes
       drakeLCMUtil
       drakeRBM
       drakeMultibodyParsers
       drakeTrajectories
       robotlocomotion-lcmtypes-cpp)
-  drake_install_libraries(pickAndPlaceCommon)
+  drake_install_libraries(pickAndPlaceDevCommon)
   drake_install_headers(
       action.h
-      pick_and_place_common.h
-      world_state.h)
-  drake_install_pkg_config_file(pick-and-place-common
-      TARGET pickAndPlaceCommon
-      LIBS -lpickAndPlaceCommon
+      pick_and_place_common.h)
+  drake_install_pkg_config_file(pick-and-place-dev-common
+      TARGET pickAndPlaceDevCommon
+      LIBS -lpickAndPlaceDevCommon
       REQUIRES drake-common drake-ik drake-kuka-iiwa-arm-common
       drake-lcm-system drake-lcmtypes-cpp drake-lcm-utils drake-rbm
       robotlocomotion-lcmtypes-cpp)
@@ -36,6 +33,7 @@ if(lcm_FOUND)
     drakeLCMTypes
     drakeLCMUtil
     drakeRigidBodyPlant
+    pickAndPlaceDevCommon
     pickAndPlaceCommon
     robotlocomotion-lcmtypes-cpp)
 endif()

drake/examples/kuka_iiwa_arm/dev/pick_and_place/action.h

@@ -6,7 +6,7 @@
 
 #include <lcm/lcm-cpp.hpp>
 
-#include "drake/examples/kuka_iiwa_arm/dev/pick_and_place/world_state.h"
+#include "drake/examples/kuka_iiwa_arm/pick_and_place/world_state.h"
 
 namespace drake {
 namespace examples {