/** * \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); }