4 tf2::Quaternion x_rot(0,0,0,1);
5 tf2::Quaternion y_rot(0,0,0.707,0.707);
6 tf2::Quaternion z_rot(0,-0.707,0,0.707);
7 std::vector<tf2::Quaternion> basic_rot{x_rot, x_rot.inverse(), y_rot, y_rot.inverse(), z_rot.inverse()};
8 ROS_INFO(
"=> 5 Basic grasp roatations (x, -x, y, -y, -z) ");
10 for(
auto&od_q: s_od_q.second){
11 od_q.second = basic_rot;
17 std::vector<tf2::Transform> trans;
18 for(
long unsigned int i = 0; i < map.size(); i++) {
19 for(
auto&s_od_q: task_space){
20 for(
auto&od_q: s_od_q.second){
21 for(
auto&q: od_q.second){
22 if (q.angle(map[i].getRotation()) < 0.349066f){
23 inv_map.push_back(tf2::Transform(q, map[i].getOrigin()).inverse());
31 ROS_INFO(
"caculated [inv_map] contains %li entrys...", inv_map.size());
35 void SimpleBaseImplementation::cloudCalculation(std::vector<tf2::Transform>& inv_map, std::map<
const std::string, std::vector<std::pair<
object_data,std::vector<tf2::Quaternion>>>>& task_space, std::map<
const std::string, std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& target_cloud){
36 ROS_INFO(
"initialyze thread implementations...");
38 for(
auto& s_od_q: task_space) {
39 std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr> clouds;
40 for(
auto& od_q: s_od_q.second) {
41 clouds.push_back(pcl::PointCloud< pcl::PointXYZ >::Ptr(
new pcl::PointCloud< pcl::PointXYZ >));
43 target_cloud.insert(std::pair<
const std::string, std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>(s_od_q.first, clouds));
46 ROS_INFO(
"start [cloud] calculation...");
48 tf2::Transform root(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
50 for(
auto& s_c: target_cloud){
51 for(
int i = 0; i < s_c.second.size();i++){
52 for(
auto& itf : inv_map){
53 for(
auto& r: task_space.at(s_c.first)[i].second){
54 tf2::Transform target(r, task_space.at(s_c.first)[i].first.pose_.getOrigin());
55 tf2::Transform base = target * (itf * root);
56 s_c.second[i]->push_back(pcl::PointXYZ(base.getOrigin().getX(), base.getOrigin().getY(), base.getOrigin().getZ()));
62 ROS_INFO(
"[cloud]calculation done...");