robossembler/runtime
7
0
Fork 0
Code Issues 5 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

22
rbs_skill_servers/src/gripper_control_action_server.cpp
View file

@ -32,36 +32,24 @@ public:
std::placeholders::_1));
publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(
"/gripper_controller/commands", 10);
join_state_subscriber =
this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
std::bind(&GripperControlActionServer::joint_state_callback, this,
std::placeholders::_1));
}
private:
rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
join_state_subscriber;
double gripper_joint_state{1.0};
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
void joint_state_callback(const sensor_msgs::msg::JointState &msg) {
gripper_joint_state = msg.position.back();
}
rclcpp_action::GoalResponse
goal_callback(const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const GripperCommand::Goal> goal) {
RCLCPP_INFO(this->get_logger(),
"goal request recieved for gripper [%.2f m]", goal->position);
"goal request recieved for gripper [%.6f m]", goal->position);
(void)uuid;
if (goal->position > 0.9 or goal->position < 0) {
return rclcpp_action::GoalResponse::REJECT;
}
// if (goal->position > 0.008 or goal->position < 0) {
// return rclcpp_action::GoalResponse::REJECT;
// }
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
@ -91,8 +79,6 @@ private:
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
return;
}
RCLCPP_INFO(this->get_logger(), "Current gripper state %f",
gripper_joint_state);
std_msgs::msg::Float64MultiArray command;

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;
// }
}