/**
* \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);
}
virtual void createSamplesMarker(const Chart &c, const Eigen::Vector3d &projected)
{
if (c.samp_chosen < 0 || no_sample_markers)
return;
for (size_t i=0; i<c.samples.rows(); ++i)
{
visualization_msgs::Marker samp;
samp.header.frame_id = mark_frame;
samp.header.stamp = ros::Time();
samp.lifetime = ros::Duration(0.5);
samp.ns = "Atlas Node(" + std::to_string(c.getId()) + ") Samples";
samp.id = i;
samp.type = visualization_msgs::Marker::SPHERE;
samp.action = visualization_msgs::Marker::ADD;
samp.scale.x = 0.01;
samp.scale.y = 0.01;
samp.scale.z = 0.01;
samp.color.a = 0.8;
if (i==c.samp_chosen){
//is winner
samp.color.r = 0.6;
samp.color.b = 0.0;
samp.color.g = 0.0;
}
else{
samp.color.r = 0.6;
samp.color.b = 0.0;
samp.color.g = 0.6;
}
samp.pose.position.x = c.samples(i,0);
samp.pose.position.y = c.samples(i,1);
samp.pose.position.z = c.samples(i,2);
std::lock_guard<std::mutex> guard(*mtx_ptr);
markers->markers.push_back(samp);
}
geometry_msgs::Point e;
geometry_msgs::Point s;
visualization_msgs::Marker proj;
proj.header.frame_id = mark_frame;
proj.header.stamp = ros::Time();
proj.lifetime = ros::Duration(0.5);
proj.ns = "Atlas Node(" + std::to_string(c.getId()) + ") Samples";
proj.id = c.samples.rows();
proj.type = visualization_msgs::Marker::ARROW;
proj.action = visualization_msgs::Marker::ADD;
s.x = c.samples(0,0);
s.y = c.samples(0,1);
s.z = c.samples(0,2);
proj.points.push_back(s);
e.x = projected[0];
e.y = projected[1];
e.z = projected[2];
proj.points.push_back(e);
proj.scale.x = 0.001;
proj.scale.y = 0.01;
proj.scale.z = 0.01;
proj.color.a = 0.4;
proj.color.r = 0.0;
proj.color.g = 0.2;
proj.color.b = 0.8;
std::lock_guard<std::mutex> guard(*mtx_ptr);
markers->markers.push_back(proj);
}