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> <launch>
<arg name="result" default="dummy/-277257222.yaml" /> <!-- specify the planning pipeline -->
<arg name="jobs" default="jobs/dummy.yaml" /> <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) -->
<arg name="db" default="false" />
<!-- Allow user to specify database location -->
<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
<!-- this is to change--> <!-- By default, we are not in debug mode -->
<rosparam command="load" file="$(find multi_cell_builder)/results/$(arg result)"/> <arg name="debug" default="false" />
<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> <!-- 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"> <!-- Choose controller manager: fake, simple, or ros_control -->
<arg name="use_rviz" value="false"/> <arg name="moveit_controller_manager" default="fake" />
<arg name="scene" value="$(arg result)" /> <!-- Set execution mode for fake execution controllers -->
</include> <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" />
<include ns="cell_routine" file="$(find ceti_double)/launch/fake_moveit_controller_manager.launch.xml" /> <group if="$(eval arg('moveit_controller_manager') == 'fake')">
<param name="move_group/capabilities" value="move_group/ExecuteTaskSolutionCapability" /> <!-- 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>
<!-- 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>
<include file="$(find ceti_double)/launch/moveit_rviz.launch"> <!-- Given the published joint states, publish tf for the robot links -->
<arg name="rviz_config" value="$(find multi_cell_builder)/launch/rviz/cell_routine.rviz" /> <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> </include>
<node pkg="multi_cell_builder" type="config_routine" name="config_routine" output="screen" required="true"> <!-- 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/moveit_grasps_config.yaml"/>
<rosparam command="load" file="$(find gb_grasp)/config/grasp_map_config.yaml"/>
</node> </node>
</launch>
<!-- 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));
mediator->connectRobots(std::move(ceti_panda));
}
//mediator->set_dirname(filename); // Run Tests
moveit_grasps_demo::GraspPipelineDemo tester;
ros::Duration(2).sleep();
tester.publishConfigScene();
tester.generateGraspMap();
tester.getVoxelizedEnv();
tester.computeConfig();
tester.build();
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