Skip to content
Snippets Groups Projects
Commit ae7d737f authored by KingMaZito's avatar KingMaZito
Browse files

first integration step

parent 04d97a0d
No related branches found
No related tags found
No related merge requests found
<launch>
<arg name="result" default="dummy/-277257222.yaml" /> <launch>
<arg name="jobs" default="jobs/dummy.yaml" /> <!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
<!-- By default, we do not start a database (it can be large) -->
<!-- this is to change--> <arg name="db" default="false" />
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <!-- Allow user to specify database location -->
<rosparam command="load" file="$(find multi_cell_builder)/mtc_templates/dummy.yaml" /> <arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
<rosparam command="load" file="$(find multi_cell_builder)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find multi_cell_builder)/descriptions/dummy2.yaml"/> <!-- By default, we are not in debug mode -->
<rosparam command="load" file="$(find multi_cell_builder)/$(arg jobs)"/> <arg name="debug" default="false" />
<rosparam ns="planning_pipelines" param="pipeline_names">["ompl"]</rosparam> <!-- By default, we will load or override the robot_description -->
<arg name="load_robot_description" default="true"/>
<include file="$(find ceti_double)/launch/demo.launch">
<arg name="use_rviz" value="false"/> <!-- Choose controller manager: fake, simple, or ros_control -->
<arg name="scene" value="$(arg result)" /> <arg name="moveit_controller_manager" default="fake" />
</include> <!-- Set execution mode for fake execution controllers -->
<arg name="fake_execution_type" default="interpolate" />
<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" />
<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" /> <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
<arg name="use_gui" default="false" />
<include file="$(find ceti_double)/launch/moveit_rviz.launch"> <arg name="use_rviz" default="true" />
<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" />
</include> <!-- 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" />
<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"/> <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> </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>
#include "mediator/abstract_mediator.h" #include "mediator/abstract_mediator.h"
#include "mediator/grasp_mediator.h" #include "mediator/grasp_mediator.h"
#include "robot/decorators/panda_decorator.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"); 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(); spinner.start();
std::shared_ptr<GraspMediator> mediator = std::make_shared<GraspMediator>(n);
auto rd = mediator->robotReader()->robotData(); // Seed random
for (int i = 0; i < rd.size() ;i++){ srand(ros::Time::now().toSec());
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)); // Run Tests
mediator->connectRobots(std::move(ceti_panda)); 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()){ return 0;
ros::spinOnce();
}
} }
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment