messages.cpp
Go to the documentation of this file.
00001 #include <handle_detector/messages.h>
00002 
00003 handle_detector::CylinderArrayMsg Messages::createCylinderArray(const std::vector<CylindricalShell> &list,
00004                                                                 std::string frame)
00005 {
00006   handle_detector::CylinderArrayMsg msg;
00007   msg.header.stamp = ros::Time::now();
00008   msg.header.frame_id = frame;
00009   msg.cylinders.resize(list.size());
00010 
00011   for (std::size_t i = 0; i < list.size(); i++)
00012   {
00013     msg.cylinders[i] = createCylinder(list[i], frame);
00014   }
00015 
00016   return msg;
00017 }
00018 
00019 handle_detector::CylinderMsg Messages::createCylinder(const CylindricalShell &shell, std::string frame)
00020 {
00021   geometry_msgs::Pose pose;
00022 
00023   // cylinder position
00024   pose.position.x = shell.getCentroid()(0);
00025   pose.position.y = shell.getCentroid()(1);
00026   pose.position.z = shell.getCentroid()(2);
00027 
00028   // create cylinder orientation from axes
00029   geometry_msgs::PoseStamped cylinder_pose_msg;
00030   Eigen::Vector3d axis_eigen = shell.getCurvatureAxis();
00031   Eigen::Vector3d normal_eigen = shell.getNormal();
00032   Eigen::Vector3d perp_eigen = normal_eigen.cross(axis_eigen);
00033   tf::Matrix3x3 rotation_matrix(perp_eigen(0), normal_eigen(0), axis_eigen(0), perp_eigen(1), normal_eigen(1),
00034                                 axis_eigen(1), perp_eigen(2), normal_eigen(2), axis_eigen(2));
00035   tf::Quaternion quaternion;
00036   rotation_matrix.getRotation(quaternion);
00037   tf::Stamped<tf::Transform> cylinder_tf_pose(tf::Transform(quaternion), ros::Time::now(), frame);
00038   tf::poseStampedTFToMsg(cylinder_tf_pose, cylinder_pose_msg);
00039   pose.orientation = cylinder_pose_msg.pose.orientation;
00040 
00041   // create message
00042   handle_detector::CylinderMsg cylinder_msg;
00043   cylinder_msg.pose = pose;
00044   cylinder_msg.radius = shell.getRadius();
00045   cylinder_msg.extent = shell.getExtent();
00046   geometry_msgs::Vector3 axis_msg;
00047   tf::vectorEigenToMsg(axis_eigen, axis_msg);
00048   geometry_msgs::Vector3 normal_msg;
00049   tf::vectorEigenToMsg(normal_eigen, normal_msg);
00050   cylinder_msg.axis = axis_msg;
00051   cylinder_msg.normal = normal_msg;
00052   return cylinder_msg;
00053 }
00054 
00055 handle_detector::HandleListMsg Messages::createHandleList(const std::vector<std::vector<CylindricalShell> > &handles,
00056                                                           std::string frame)
00057 {
00058   handle_detector::HandleListMsg msg;
00059   msg.header.stamp = ros::Time::now();
00060   msg.header.frame_id = frame;
00061   msg.handles.resize(handles.size());
00062 
00063   for (std::size_t i = 0; i < handles.size(); i++)
00064     msg.handles[i] = createCylinderArray(handles[i], frame);
00065 
00066   return msg;
00067 }


handle_detector
Author(s):
autogenerated on Thu Jun 6 2019 17:36:23