diff --git a/launch/config_routine.launch b/launch/config_routine.launch index 3678cfd10119d452adf23c0e7f81596bae009a37..dc76e32ffb0b1b7c7e9152548c1d982c20871b34 100644 --- a/launch/config_routine.launch +++ b/launch/config_routine.launch @@ -1,33 +1,85 @@ -<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> diff --git a/src/config_routine.cpp b/src/config_routine.cpp index 257ca5fdfc59a9c148b5e974903d7ece753998cb..8c228c457b7f86c598d6dde949db95e2a1376709 100644 --- a/src/config_routine.cpp +++ b/src/config_routine.cpp @@ -1,30 +1,31 @@ #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