Select Git revision
grasp_mediator.cpp
grasp_mediator.cpp 9.81 KiB
#include "mediator/grasp_mediator.h"
GraspMediator::GraspMediator(std::shared_ptr<ros::NodeHandle> const& nh)
: MoveitMediator(nh)
, planning_scene_monitor_(std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"))
{
if (planning_scene_monitor_->getPlanningScene()){
planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
"planning_scene");
planning_scene_monitor_->getPlanningScene()->setName("planning_scene");
} else {
ROS_ERROR_STREAM_NAMED("test", "Planning scene not configured");
return;
}
visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
visual_tools_->loadMarkerPub();
visual_tools_->loadRemoteControl();
visual_tools_->setPlanningSceneMonitor(planning_scene_monitor_);
visual_tools_->loadRobotStatePub("/display_robot_state");
visual_tools_->setRobotStateTopic("/robot_state_publisher");
visual_tools_->loadTrajectoryPub("/display_planned_path");
visual_tools_->loadSharedRobotState();
visual_tools_->enableBatchPublishing();
visual_tools_->deleteAllMarkers();
visual_tools_->removeAllCollisionObjects();
//visual_tools_->hideRobot();
visual_tools_->trigger();
// Publish the global frame
visual_tools_->publishAxis(Eigen::Isometry3d::Identity());
visual_tools_->trigger();
}
void GraspMediator::mediate() {
setPanel();
publishTables();
generateGraspMap();
ros::waitForShutdown();
}
void GraspMediator::generateGraspMap() {
auto cd = cuboid_reader_->cuboidBin();
//std::vector<std::string> objs = {"bottle1", "bottle2"};
for (int i = 0; i < cd.size(); i ++){
std::stringstream ss;
ss << "box_" << i;
moveit_msgs::CollisionObject item;
item.id = ss.str();
item.header.frame_id = "world";
item.header.stamp = ros::Time::now();
item.primitives.resize(1);
item.primitives[0].type = item.primitives[0].BOX;
item.primitives[0].dimensions.resize(3);
item.primitives[0].dimensions[0] = cd[i].x_depth;
item.primitives[0].dimensions[1] = cd[i].y_width;
item.primitives[0].dimensions[2] = cd[i].z_heigth;
item.primitive_poses.resize(1);
item.primitive_poses[0].position.x = cd[i].Pose.position.x;
item.primitive_poses[0].position.y = cd[i].Pose.position.y;
item.primitive_poses[0].position.z = cd[i].Pose.position.z;
item.primitive_poses[0].orientation.x = cd[i].Pose.orientation.x; item.primitive_poses[0].orientation.y = cd[i].Pose.orientation.y;
item.primitive_poses[0].orientation.z = cd[i].Pose.orientation.z;
item.primitive_poses[0].orientation.w = cd[i].Pose.orientation.w;
item.operation = item.ADD;
psi_->applyCollisionObject(item);
// Could also safe id's somewhere
}
ROS_INFO("Debug1!");
auto arm_jmg_ = robot_model_->getJointModelGroup(robots_.at("panda_arm1")->name());
ROS_INFO("Debug1.5!");
auto voxel_manager_ = std::make_shared<VoxelManager>(*nh_,"",visual_tools_,planning_scene_monitor_);
ROS_INFO("%f", robots_.at("panda_arm1")->tf().getOrigin().getZ());
ROS_INFO("Debug2!");
Cuboid voxelSpace("VS");
voxelSpace.Pose.position.x= robots_.at("panda_arm1")->tf().getOrigin().getX();
voxelSpace.Pose.position.y= robots_.at("panda_arm1")->tf().getOrigin().getY();
voxelSpace.Pose.position.z= robots_.at("panda_arm1")->tf().getOrigin().getZ()*2 + 0.1f;
voxelSpace.Pose.orientation.x = robots_.at("panda_arm1")->tf().getRotation().getX();
voxelSpace.Pose.orientation.y = robots_.at("panda_arm1")->tf().getRotation().getY();
voxelSpace.Pose.orientation.z = robots_.at("panda_arm1")->tf().getRotation().getZ();
voxelSpace.Pose.orientation.w = robots_.at("panda_arm1")->tf().getRotation().getW();
voxelSpace.x_depth = 0.3f;
voxelSpace.y_width = 0.3f;
voxelSpace.z_heigth = 0.2f;
voxel_manager_->setVerboseLevel(0);
voxel_manager_->setVoxelSize(0.02f);
voxel_manager_->setVoxelSpace(voxelSpace);
ROS_INFO("Debug2.5!");
XmlRpc::XmlRpcValue data;
nh_->getParam("/", data);
std::regex rx("panda_arm([0-9]+)");
std::smatch match;
std::regex_match(robots_.at("panda_arm1")->name(), match, rx);
data["base_link"]= "world";
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["end_effector_name"] = robots_.at("panda_arm1")->map().at("eef_name").c_str();
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["end_effector_type"] = "finger";
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_resolution"] = 0.001f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["angle_resolution"] = 45;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_max_depth"] = 0.035f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_min_depth"] = 0.01f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_depth_resolution"] = 0.005f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["approach_distance_desired"] = 0.05f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["retreat_distance_desired"] = 0.05f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["lift_distance_desired"] = 0.02f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_padding_on_approach"] = 0.005f;
std::stringstream ss;
ss << "panda_" << match[1] << "_tool_center_point";
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["define_tcp_by_name"] = false;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_name"] = ss.str().c_str();
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][0] = 0;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][1] = 0;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][2] = -0.105f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][3] = 0;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][4] = 0;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["tcp_to_eef_mount_transform"][5] = -0.7853f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["pregrasp_time_from_start"] = 0;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_time_from_start"] = 0;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["pregrasp_posture"][0] = 0.04f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["grasp_posture"][0] = 0;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["joints"][0] = robots_.at("panda_arm1")->mgiHand()->getJoints()[1].c_str();
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["max_finger_width"] = 0.085f; data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["min_finger_width"] = 0.06f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["max_grasp_width"] = 0.08f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["gripper_finger_width"] = 0.015f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["active_suction_range_x"] = 0.022f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["active_suction_range_y"] = 0.012f;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["suction_rows_count"] = 2;
data[robots_.at("panda_arm1")->map().at("eef_name").c_str()]["suction_cols_count"] = 2;
nh_->setParam("/", data);
MapGenerator::MapGeneratorPtr grasp_map_generator_ = std::make_shared<MapGenerator>(*nh_,
visual_tools_,
planning_scene_monitor_,
robots_.at("panda_arm1")->map().at("eef_name"),
robots_.at("panda_arm1")->name(),
arm_jmg_,
robots_.at("panda_arm1")->mgi()->getCurrentState(),
voxel_manager_);
auto cb = cuboid_reader_->cuboidBin();
ROS_INFO("Debug3!");
std::vector<GraspMap> maps;
Cuboid workspace("WS");
workspace.Pose.position.x= robots_.at("panda_arm1")->tf().getOrigin().getX();
workspace.Pose.position.y= robots_.at("panda_arm1")->tf().getOrigin().getY();
workspace.Pose.position.z= robots_.at("panda_arm1")->tf().getOrigin().getZ()*2 + 0.02f;
workspace.Pose.orientation.x = robots_.at("panda_arm1")->tf().getRotation().getX();
workspace.Pose.orientation.y = robots_.at("panda_arm1")->tf().getRotation().getY();
workspace.Pose.orientation.z = robots_.at("panda_arm1")->tf().getRotation().getZ();
workspace.Pose.orientation.w = robots_.at("panda_arm1")->tf().getRotation().getW();
workspace.x_depth = 0.2f;
workspace.y_width = 0.2f;
workspace.z_heigth = 0.04f;
ROS_INFO("Debug4!");
double cube_clearence, translation_rate, rotation_rate, max_rotation;
cube_clearence = 0.01f;
translation_rate = 0.03f;
rotation_rate = 45.f * (M_PI/180.f);
max_rotation = 90.f * (M_PI/180.f);
for (auto& object: cb) {
GraspMap map(workspace,object,cube_clearence,translation_rate,rotation_rate,max_rotation);
maps.push_back(map);
}
ROS_INFO("Debug5!");
// Create Maps
{
ROS_INFO("creating map ...");
auto start_time = ros::Time::now();
for (GraspMap &m: maps) { grasp_map_generator_->SampleMap(m); }
double durration = (start_time - ros::Time::now()).toSec();
ROS_INFO_STREAM(maps.size() << " GraspMaps created in " << durration << " seconds");
}
ROS_INFO("Debug6!");
}