first integration step

launch/config_routine.launch

-<launch>    
-    <arg name="result" default="dummy/-277257222.yaml" />
-    <arg name="jobs" default="jobs/dummy.yaml" />
-
-    <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
-
-    <!-- this is to change-->
-    <rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/>
-    <rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" />
-    <rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
-    <rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/>
-    <rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/>
-
-    <rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam>
-
-    <include file="$(find ceti_double)/launch/demo.launch"> 
-        <arg name="use_rviz" value="false"/>
-        <arg name="scene" value="$(arg result)" />
-    </include> 
-    
-    <include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
-    <param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" />
-
-    <include file="$(find ceti_double)/launch/moveit_rviz.launch">
-        <arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
-    </include>
-
-    <node pkg="multi_cell_builder" type="config_routine" name="config_routine" output="screen" required="true">
-        <rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
+
+<launch>
+  <!-- specify the planning pipeline -->
+  <arg name="pipeline" default="ompl" />
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!-- By default, we will load or override the robot_description -->
+  <arg name="load_robot_description" default="true"/>
+
+  <!-- Choose controller manager: fake, simple, or ros_control -->
+  <arg name="moveit_controller_manager" default="fake" />
+  <!-- Set execution mode for fake execution controllers -->
+  <arg name="fake_execution_type" default="interpolate" />
+
+  <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
+  <arg name="use_gui" default="false" />
+  <arg name="use_rviz" default="true" />
+
+  <!-- If needed, broadcast static tf for robot root -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0" />
+
+  <group if="$(eval arg('moveit_controller_manager') == 'fake')">
+    <!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
+         MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
     </node>
-</launch>
+    <!-- If desired, a GUI version is available allowing to move the simulated robot around manually
+         This corresponds to moving around the real robot without the use of MoveIt. -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
+      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+    </node>
+
+    <!-- Given the published joint states, publish tf for the robot links -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+  </group>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(find panda_moveit_config)/launch/move_group.launch">
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="pipeline" value="$(arg pipeline)"/>
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(find panda_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+  <!-- Load Rviz -->
+  <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+	args="-d $(find moveit_grasps)/launch/rviz/grasps.rviz" output="screen">
+	<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+  </node>
 
-    
+
+  <!-- Start the demo -->
+  <node name="config_routine"  pkg="multi_cell_builder"
+  type="config_routine" output="screen">
+    <param name="gripper" value="two_finger"/>
+    <param name="ee_group_name" value="hand"/>
+    <param name="planning_group_name" value="panda_arm_hand"/>
+    <rosparam command="load" file="$(find gb_grasp)/config_robot/panda_grasp_data.yaml"/>
+    <rosparam command="load" file="$(find gb_grasp)/config/moveit_grasps_config.yaml"/>
+    <rosparam command="load" file="$(find gb_grasp)/config/grasp_map_config.yaml"/>
+  </node>
+
+  <!-- Planning Functionality -->
+  <arg name="planner" default="ompl" />
+  <include ns="moveit_grasps_demo" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="$(arg planner)" />
+  </include>
+
+</launch>

src/config_routine.cpp

 #include "mediator/abstract_mediator.h"
 #include "mediator/grasp_mediator.h"
 #include "robot/decorators/panda_decorator.h"
+#include <gb_grasp/GraspPipelineDemo.h>
 
 
 
-int main(int argc, char **argv){
+int main(int argc, char *argv[]) {
+
     ros::init(argc, argv, "config_routine");
-    std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
 
-    ros::AsyncSpinner spinner(1);
+    // Allow the action server to recieve and send ros messages
+    ros::AsyncSpinner spinner(2);
     spinner.start();
 
-    std::shared_ptr<GraspMediator> mediator = std::make_shared<GraspMediator>(n);
 
-    auto rd = mediator->robotReader()->robotData();
-    for (int i = 0; i < rd.size() ;i++){
-        std::unique_ptr<AbstractRobot> ceti = std::make_unique<CetiRobot>(rd[i].name_, rd[i].pose_, rd[i].size_);
-        std::unique_ptr<PandaDecorator> ceti_panda = std::make_unique<PandaDecorator>(std::move(ceti));
-        mediator->connectRobots(std::move(ceti_panda));
-    }
+    // Seed random
+    srand(ros::Time::now().toSec());
+
+        // Run Tests
+    moveit_grasps_demo::GraspPipelineDemo tester;
+    ros::Duration(2).sleep();
+    tester.publishConfigScene();
+    tester.generateGraspMap();
+    tester.getVoxelizedEnv();
+    tester.computeConfig();
+    tester.build();
 
-    //mediator->set_dirname(filename);
-    
-    mediator->mediate();
 
-    while (ros::ok()){
-        ros::spinOnce();
-    }
+    return 0;
 }
\ No newline at end of file