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

...

parent 7f1213cb
Branches
No related tags found
No related merge requests found
...@@ -2,10 +2,7 @@ ...@@ -2,10 +2,7 @@
#include <regex> #include <regex>
#include <gb_grasp/MapConfigLoader.h> #include <gb_grasp/MapConfigLoader.h>
#include <moveit/robot_state/conversions.h> #include <moveit/robot_state/conversions.h>
<<<<<<< HEAD
=======
#include "impl/wing_moveit_decorator.h" #include "impl/wing_moveit_decorator.h"
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) { void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
...@@ -14,10 +11,6 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) { ...@@ -14,10 +11,6 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
auto mr = dynamic_cast<Moveit_robot*>(robot); auto mr = dynamic_cast<Moveit_robot*>(robot);
manipulate_grasp_data(mr); manipulate_grasp_data(mr);
<<<<<<< HEAD
manipulate_mapspace();
=======
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
auto map_gen = std::make_shared<MapGenerator>(*nh_, visual_tools_, planning_scene_monitor_, mr->map().at("eef_name"), mr->name().c_str(), robot_model_->getJointModelGroup(mr->name().c_str()), mgi().get()->getCurrentState(), voxel_manager_); auto map_gen = std::make_shared<MapGenerator>(*nh_, visual_tools_, planning_scene_monitor_, mr->map().at("eef_name"), mr->name().c_str(), robot_model_->getJointModelGroup(mr->name().c_str()), mgi().get()->getCurrentState(), voxel_manager_);
double min_quality, max_candidates; double min_quality, max_candidates;
...@@ -26,13 +19,10 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) { ...@@ -26,13 +19,10 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality); rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"min_quality",min_quality);
rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"max_candidate_count",max_candidates); rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"max_candidate_count",max_candidates);
rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"top_grasps_only",top_only); rosparam_shortcuts::get(grasp_ns_.c_str(),*nh_,"top_grasps_only",top_only);
<<<<<<< HEAD
=======
ROS_INFO("min_quality: %f", min_quality); ROS_INFO("min_quality: %f", min_quality);
ROS_INFO("max_candidate_count: %f", max_candidates); ROS_INFO("max_candidate_count: %f", max_candidates);
ROS_INFO("top_grasps_only: %f", top_only); ROS_INFO("top_grasps_only: %f", top_only);
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
map_gen->SetQuality(min_quality); map_gen->SetQuality(min_quality);
...@@ -42,17 +32,6 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) { ...@@ -42,17 +32,6 @@ void Moveit_grasp_mediator::connect_robots(Abstract_robot* robot) {
} }
<<<<<<< HEAD
void Moveit_grasp_mediator::manipulate_mapspace(){
std::vector<float> pos_voxel_space;
std::vector<float> pos_mapspace;
nh_->getParam("/voxel_space/pos", pos_voxel_space);
nh_->getParam("/mapspace/pos", pos_mapspace);
pos_mapspace[2]= 0.91f;
pos_voxel_space[2]= 0.91f;
nh_->setParam("/voxel_space/pos", pos_voxel_space);
nh_->setParam("/mapspace/pos", pos_mapspace);
=======
void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){ void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3& size){
double r,p,y; double r,p,y;
std::vector<double> rot_mapspace(3,0); std::vector<double> rot_mapspace(3,0);
...@@ -65,7 +44,6 @@ void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3 ...@@ -65,7 +44,6 @@ void Moveit_grasp_mediator::manipulate_mapspace(tf2::Transform& tf, tf2::Vector3
//nh_->setParam("/voxel_space/dim", std::vector<float>({0.3f,0.3f,0.2f})); //nh_->setParam("/voxel_space/dim", std::vector<float>({0.3f,0.3f,0.2f}));
//nh_->setParam("/voxel_space/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), tf.getOrigin().getZ()})); //nh_->setParam("/voxel_space/pos", std::vector<double>({tf.getOrigin().getX(), tf.getOrigin().getY(), tf.getOrigin().getZ()}));
//nh_->setParam("/voxel_space/rot", std::vector<double>({r,p,y})); //nh_->setParam("/voxel_space/rot", std::vector<double>({r,p,y}));
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
} }
void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) { void Moveit_grasp_mediator::manipulate_grasp_data(Moveit_robot* robot) {
...@@ -131,11 +109,7 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome ...@@ -131,11 +109,7 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome
double& y_width, double& z_height) double& y_width, double& z_height)
{ {
// Generate random cuboid // Generate random cuboid
<<<<<<< HEAD
double xmin = 0.5;
=======
double xmin = 0.1; double xmin = 0.1;
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
double xmax = 0.7; double xmax = 0.7;
double ymin = -0.25; double ymin = -0.25;
double ymax = 0.25; double ymax = 0.25;
...@@ -166,36 +140,7 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome ...@@ -166,36 +140,7 @@ bool Moveit_grasp_mediator::generateRandomCuboid(std::string& object_name, geome
void Moveit_grasp_mediator::mediate(){ void Moveit_grasp_mediator::mediate(){
visual_tools_->deleteAllMarkers(); visual_tools_->deleteAllMarkers();
<<<<<<< HEAD
/*
MapConfigLoader map_config(*nh_, grasp_ns_);
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
map_config.SetObjects(cbd);
map_config.Create(grasp_maps_);
ROS_INFO_STREAM("number of maps: "<< grasp_maps_.size());
{
ROS_INFO("creating map ...");
auto start_time = ros::Time::now();
for (auto* r : robots_){
auto mr = dynamic_cast<Moveit_robot*>(r);
for (GraspMap &m: grasp_maps_) { mr->grasp_map_generator()->SampleMap(m); }
double durration = (start_time - ros::Time::now()).toSec();
ROS_INFO_STREAM(grasp_maps_.size() << " GraspMaps created in " << durration << " seconds");
}
}
visual_tools_->setAlpha(0.1);
voxel_manager_->Voxelize();
auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
for(auto&o:cod){
voxel_manager_->GetCuboidCollisions(o,voxel_environment_);
}
voxel_manager_->Voxelize(true);
*/
=======
ROS_INFO("ns is %s", grasp_ns_.c_str()); ROS_INFO("ns is %s", grasp_ns_.c_str());
MapConfigLoader map_config(*nh_, grasp_ns_.c_str()); MapConfigLoader map_config(*nh_, grasp_ns_.c_str());
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin(); auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
...@@ -275,7 +220,6 @@ void Moveit_grasp_mediator::mediate(){ ...@@ -275,7 +220,6 @@ void Moveit_grasp_mediator::mediate(){
} }
voxel_manager_->Voxelize(true); voxel_manager_->Voxelize(true);
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
// ----------------------------------- // -----------------------------------
// Generate random object to grasp // Generate random object to grasp
...@@ -490,11 +434,7 @@ bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_ ...@@ -490,11 +434,7 @@ bool Moveit_grasp_mediator::planPreApproach(const robot_state::RobotState& goal_
double tolerance_above = 0.01; double tolerance_above = 0.01;
double tolerance_below = 0.01; double tolerance_below = 0.01;
// req.planner_id = "RRTConnectkConfigDefault"; // req.planner_id = "RRTConnectkConfigDefault";
<<<<<<< HEAD
req.group_name = "panda_arm1";
=======
req.group_name = "panda_1_arm1"; req.group_name = "panda_1_arm1";
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
req.num_planning_attempts = 5; req.num_planning_attempts = 5;
req.allowed_planning_time = 1.5; req.allowed_planning_time = 1.5;
moveit_msgs::Constraints goal = moveit_msgs::Constraints goal =
...@@ -564,13 +504,8 @@ void Moveit_grasp_mediator::scene_setup(){ ...@@ -564,13 +504,8 @@ void Moveit_grasp_mediator::scene_setup(){
auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin(); auto cbd = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_bin();
auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle(); auto cod = static_cast<Cuboid_reader*>(cuboid_reader_.get())->cuboid_obstacle();
<<<<<<< HEAD
ROS_INFO("%i die cuboids", cbd.size());
ROS_INFO("%i cuboids", cod.size());
=======
ROS_INFO("%i die cuboids", cbd.size()); ROS_INFO("%i die cuboids", cbd.size());
ROS_INFO("%i cuboids", cod.size()); ROS_INFO("%i cuboids", cod.size());
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
for(auto&o:cod){ for(auto&o:cod){
o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY); o.publishCOwait(visual_tools_,planning_scene_monitor_,rviz_visual_tools::GREY);
...@@ -579,13 +514,8 @@ void Moveit_grasp_mediator::scene_setup(){ ...@@ -579,13 +514,8 @@ void Moveit_grasp_mediator::scene_setup(){
for(auto&o:cbd){ for(auto&o:cbd){
//o.publish(visual_tools_,rviz_visual_tools::GREEN); //o.publish(visual_tools_,rviz_visual_tools::GREEN);
<<<<<<< HEAD
psi_->addCollisionObjects({o.getCoMsg()});
}
=======
psi_->addCollisionObjects({o.getCoMsg()}); psi_->addCollisionObjects({o.getCoMsg()});
} }
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
visual_tools_->publishRobotState(mgi_->getCurrentState()); visual_tools_->publishRobotState(mgi_->getCurrentState());
visual_tools_->trigger(); visual_tools_->trigger();
...@@ -615,11 +545,8 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf ...@@ -615,11 +545,8 @@ Moveit_grasp_mediator::Moveit_grasp_mediator(std::vector<std::vector<tf2::Transf
visual_tools_->trigger(); visual_tools_->trigger();
nh_->getParam("/grasps_ns", grasp_ns_); nh_->getParam("/grasps_ns", grasp_ns_);
<<<<<<< HEAD
=======
grasp_ns_ = "grasps_ns"; grasp_ns_ = "grasps_ns";
ROS_INFO("ns is %s", grasp_ns_.c_str()); ROS_INFO("ns is %s", grasp_ns_.c_str());
>>>>>>> 4694ff9 (acm merge done, ferdinand done, working on tasks)
planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter"); planning_pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, *nh_, "planning_plugin", "request_adapter");
voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_); voxel_manager_ = std::make_shared<VoxelManager>(*nh_, grasp_ns_.c_str() ,visual_tools_,planning_scene_monitor_);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment