robossembler/runtime
7
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

rbs_utils in env_manager and clear code

Browse source
This commit is contained in:
Ilya Uraev 2023-12-14 12:00:35 +03:00
parent 0588a5f6c6
commit 34c8961723
23 changed files with 536 additions and 237 deletions
Download patch file Download diff file Expand all files Collapse all files

24
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -38,11 +38,6 @@ void GetGraspPlacePoseServer::handleServer(
response) {
std::string object_name =
request->object_name + "_place"; // TODO: replace with better name
// Load place pose from TF2
auto d = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example");
d->setTfData();
// rbs_utils::processAsmFolderName("asp-example");
try {
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
@ -66,10 +61,7 @@ void GetGraspPlacePoseServer::handleServer(
grasp_pose_tf.transform.translation.x,
grasp_pose_tf.transform.translation.y,
grasp_pose_tf.transform.translation.z);
// TODO: Here need to check the parameter
// We can use another parameter library from PickNik
// grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
// .as_double_array();
RCLCPP_DEBUG(this->get_logger(),
"\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
request->grasp_direction.x, request->grasp_direction.y,
@ -84,8 +76,7 @@ void GetGraspPlacePoseServer::handleServer(
Eigen::Vector3d scale_grasp(0, 0, 0.10);
Eigen::Vector3d scale_place(0, 0, 0.15);
auto path = std::getenv("RBS_ASSEMBLY_PATH");
RCLCPP_ERROR_STREAM(this->get_logger(), path);
response->grasp =
collectPose(grasp_pose, request->grasp_direction, scale_grasp);
response->place =
@ -128,13 +119,4 @@ std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collectPose(
poses.push_back(tf2::toMsg(postGraspPose));
return poses;
}
// std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::getPlacePoseJson(const nlohmann::json& json)
// {
// std::vector<geometry_msgs::msg::Pose> place_pose;
// auto env_path = std::getenv("PATH");
// RCLCPP_INFO_STREAM(this->get_logger(), env_path);
// return place_pose;
// }
}