Commit 8a554737 authored by dla.adolfsson@gmail.com's avatar dla.adolfsson@gmail.com
Browse files

added missing visualization

parent 2fcb1564
......@@ -106,13 +106,53 @@ visualization_msgs::MarkerArray getMarkerArrayFromNDTCalibScanPairs(const std::v
}
return m;
}
visualization_msgs::Marker getMarkerCylinder(const Eigen::Affine3d &T,
int id, int color,
double length, double radius,
const std::string &ns) {
visualization_msgs::Marker m;
assignDefault(m);
assignColor(m, color);
m.ns = ns;
m.type = visualization_msgs::Marker::CYLINDER;
m.action = visualization_msgs::Marker::ADD;
m.id = id;
m.scale.x = radius; m.scale.y = radius; m.scale.z = length;
tf::poseEigenToMsg(T, m.pose);
return m;
}
void appendMarkerArray(visualization_msgs::MarkerArray &array, const visualization_msgs::MarkerArray &add) {
for (size_t i = 0; i < add.markers.size(); i++) {
array.markers.push_back(add.markers[i]);
}
}
/// Draw an x,y,z coordsystem given an affine3d.
visualization_msgs::MarkerArray getMarkerFrameAffine3d(const Eigen::Affine3d &T, const std::string &ns, double length, double radius) {
visualization_msgs::MarkerArray m;
// X
{
Eigen::Affine3d T_x =
Eigen::Translation3d(length / 2.0, 0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY());
T_x = T * T_x;
m.markers.push_back(getMarkerCylinder(T_x, 0, 0, length, radius, ns));
}
// Y
{
Eigen::Affine3d T_y =
Eigen::Translation3d(0, length / 2.0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX());
T_y = T * T_y;
m.markers.push_back(getMarkerCylinder(T_y, 1, 1, length, radius, ns));
}
// Z
{
Eigen::Affine3d T_z = Eigen::Translation3d(0, 0, length / 2.0) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
T_z = T * T_z;
m.markers.push_back(getMarkerCylinder(T_z, 2, 2, length, radius, ns));
}
return m;
}
visualization_msgs::MarkerArray getMarkerArrayRelFromNDTCalibScanPair(const NDTCalibScanPair &pair, const Eigen::Affine3d &Ts) {
visualization_msgs::MarkerArray m;
......
......@@ -42,8 +42,9 @@ public:
}
bool getTransformationForTime(ros::Time t0, const std::string &frame_id,Eigen::Affine3d &T){
bool getTransformationForTime(ros::Time t0, const std::string &frame_id, Eigen::Affine3d &T){
std::string str;
std::cout<<"Get transformation from"<<fixedframe_<<" to "<<frame_id<<std::endl;
if (!transformer_.canTransform(fixedframe_, frame_id, t0, &str)) {
return false;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment