/**
* \brief create a branch marker between two nodes and store it into marker array
*/
virtual void createBranchMarker(const Chart &child, const Chart &parent)
{
if (!markers){
ROS_WARN_THROTTLE(30,"[ExplorerBase::%s]\tNo marker array to update is provided, visualization is disabled.",__func__);
return;
}
geometry_msgs::Point e;
geometry_msgs::Point s;
visualization_msgs::Marker branch;
branch.header.frame_id = mark_frame;
branch.header.stamp = ros::Time();
branch.lifetime = ros::Duration(0.5);
branch.ns = "Atlas Branches";
//need to know the branch id, too bad branches don't have it.
//Lets use Cantor pairing function: 0.5(a+b)(a+b+1)+b
branch.id = 0.5*(child.getId() + parent.getId())*(child.getId()+parent.getId()+1) + parent.getId();
branch.type = visualization_msgs::Marker::LINE_STRIP;
branch.action = visualization_msgs::Marker::ADD;
s.x = child.getCenter()[0];
s.y = child.getCenter()[1];
s.z = child.getCenter()[2];
branch.points.push_back(s);
e.x = parent.getCenter()[0];
e.y = parent.getCenter()[1];
e.z = parent.getCenter()[2];
branch.points.push_back(e);
branch.scale.x = 0.005;
branch.color.a = 1.0;
branch.color.r = 0.0;
branch.color.g = 0.0;
branch.color.b = 0.9;
std::lock_guard<std::mutex> guard(*mtx_ptr);
markers->markers.push_back(branch);
}
/**
* \brief create a marker from a chart and stores it into markers array
*/
virtual void createNodeMarker(const Chart &c)
{
if (!markers){
ROS_WARN_THROTTLE(30,"[ExplorerBase::%s]\tNo marker array to update is provided, visualization is disabled.",__func__);
return;
}
visualization_msgs::Marker disc;
disc.header.frame_id = mark_frame;
disc.header.stamp = ros::Time();
disc.lifetime = ros::Duration(0.5);
disc.ns = "Atlas Nodes";
disc.id = c.getId();
disc.type = visualization_msgs::Marker::CYLINDER;
disc.action = visualization_msgs::Marker::ADD;
disc.scale.x = c.getRadius()*2;
disc.scale.y = c.getRadius()*2;
disc.scale.z = 0.001;
disc.color.a = 0.75;
disc.color.r = 0.3;
disc.color.b = 0.35;
disc.color.g = 0.5;
Eigen::Matrix3d rot;
rot.col(0) = c.getTanBasisOne();
rot.col(1) = c.getTanBasisTwo();
rot.col(2) = c.getNormal();
Eigen::Quaterniond q(rot);
q.normalize();
disc.pose.orientation.x = q.x();
disc.pose.orientation.y = q.y();
disc.pose.orientation.z = q.z();
disc.pose.orientation.w = q.w();
disc.pose.position.x = c.getCenter()[0];
disc.pose.position.y = c.getCenter()[1];
disc.pose.position.z = c.getCenter()[2];
std::lock_guard<std::mutex> guard(*mtx_ptr);
markers->markers.push_back(disc);
}