void scigraphics::painter::clearBordersArea()
{
if ( ! ableToDraw() )
return;
const int Shift = 10;
wpoint Point1( -Shift, -Shift );
wpoint Point2( Indents.left(), Drawer->height() + Shift );
Drawer->eraseRectangle( wrectangle(Point1,Point2) );
wpoint Point3( Drawer->width() + Shift, Drawer->height() - Indents.down() );
Drawer->eraseRectangle( wrectangle(Point2,Point3) );
wpoint Point4( Drawer->width() - Indents.right(), -Shift );
Drawer->eraseRectangle( wrectangle(Point3,Point4) );
wpoint Point5( -Shift, Indents.up() );
Drawer->eraseRectangle( wrectangle(Point4,Point5) );
}
void CPolygon::pushPoint(const Vector2& point)
{
mCGALPolygon.push_back(Point1(point.x, point.y));
}
bool CPolygon::ptInPolygon(const Vector2& point) const
{
return (mCGALPolygon.bounded_side(Point1(point.x, point.y)) == CGAL::ON_BOUNDED_SIDE);
}
void Drawer::DrawText(std::string& aTexte)
{
Point Point1 (100,100);
FacesMap::iterator IterFont;
cairo_text_extents_t te;
cairo_font_face_t* Cairo_font_face = NULL;
for (IterFont = mFacesMap.begin(); IterFont != mFacesMap.end(); ++IterFont)
{
cairo_user_data_key_t Key;
FT_Face FaceFind = IterFont->second;
if (FaceFind)
{
Cairo_font_face = cairo_ft_font_face_create_for_ft_face (FaceFind,0);
cairo_font_face_set_user_data (Cairo_font_face, &Key, FaceFind, NULL);
cairo_set_font_size (mCairoDC, 75);
cairo_set_source_rgb (mCairoDC, 0.0, 0.0, 0.0);
MoveTo(Point1);
std::string StrTmp = IterFont->first;
StrTmp += "||";
cairo_ft_font_face_set_synthesize (Cairo_font_face, CAIRO_FT_SYNTHESIZE_BOLD);
cairo_ft_font_face_set_synthesize (Cairo_font_face, CAIRO_FT_SYNTHESIZE_OBLIQUE);
cairo_set_font_face(mCairoDC, Cairo_font_face);
cairo_text_path (mCairoDC, StrTmp.c_str());
cairo_fill(mCairoDC);
cairo_text_extents(mCairoDC,StrTmp.c_str(),&te);
Point1.X += (te.width + 10);
MoveTo(Point1);
cairo_text_path (mCairoDC, IterFont->first.c_str());
cairo_fill(mCairoDC);
Point1.X -= (te.width + 10);
Point1.Y += 100;
cairo_font_face_set_user_data (Cairo_font_face, &Key, NULL, NULL);
cairo_font_face_destroy(Cairo_font_face);
}
}
cairo_user_data_key_t KeyFontSystem;
if (mFontSystem)
{
Cairo_font_face = cairo_ft_font_face_create_for_ft_face (mFontSystem,0);
cairo_font_face_set_user_data (Cairo_font_face, &KeyFontSystem, mFontSystem, NULL);
cairo_set_font_size (mCairoDC, 75);
cairo_set_source_rgb (mCairoDC, 0.0, 0.0, 0.0);
MoveTo(Point1);
cairo_set_font_face(mCairoDC, Cairo_font_face);
cairo_text_path (mCairoDC, "FontSystem");
cairo_fill(mCairoDC);
cairo_font_face_set_user_data (Cairo_font_face, &KeyFontSystem, NULL, NULL);
cairo_font_face_destroy(Cairo_font_face);
}
Point1.X -= (100);
Point1.Y += 100;
cairo_user_data_key_t KeyFontTest;
if (mFontTest)
{
Cairo_font_face = cairo_ft_font_face_create_for_ft_face (mFontTest,0);
cairo_font_face_set_user_data (Cairo_font_face, &KeyFontTest, mFontTest, NULL);
cairo_set_font_size (mCairoDC, 75);
cairo_set_source_rgb (mCairoDC, 0.0, 0.0, 0.0);
MoveTo(Point1);
cairo_set_font_face(mCairoDC, Cairo_font_face);
cairo_text_path (mCairoDC, "FontTest");
cairo_fill(mCairoDC);
cairo_font_face_set_user_data (Cairo_font_face, &KeyFontTest, NULL, NULL);
cairo_font_face_destroy(Cairo_font_face);
}
}
Point1 __gun__()
{
return Point1();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ParseVito");
ros::NodeHandle node("~");
// VitoSkelethon vito_skelethon;
LineCollisions LineCollisionsLocal;
double spin_rate = 1000;
ros::param::get("~spin_rate",spin_rate);
ROS_DEBUG( "Spin Rate %lf", spin_rate);
ros::Rate rate(spin_rate);
std::string base("world");
double threshold = .15;
std::vector<std::string> list_brach_names;
list_brach_names.push_back("left_arm/");
list_brach_names.push_back("right_arm/");
node.param<std::string>("base", base, "world");
node.param<double>("threshold", threshold, .15);
ROS_INFO_STREAM("Using Base: " << base.c_str());
ROS_INFO_STREAM("Threshold: " << threshold);
tf::TransformListener tf_listener;
std::vector<std::vector<std::string>> links_all_branch;
ros::Publisher list_pub = node.advertise<visualization_msgs::Marker>("skelethon_lines", 10);
ros::Publisher point_pub = node.advertise<visualization_msgs::Marker>("skelethon_point", 10);
ros::Publisher collisions_lines_pub = node.advertise<visualization_msgs::Marker>("skelethon_collision_lines", 10);
ros::Publisher collisions_points_pub = node.advertise<visualization_msgs::Marker>("skelethon_collision_point", 10);
for (unsigned int i = 0; i< list_brach_names.size(); i++)
{
std::vector<std::string> links_in_brach = getSkelethonPoints(node, list_brach_names[i]);
links_all_branch.push_back(links_in_brach);
}
while (node.ok())
{
tf::StampedTransform trans, trans2;
std::vector<std::vector<LineSkelethon>> List_lines_in_all_chains, lines_and_collisions_multiple;
std::vector<LineSkelethon> lines_and_collisions;
for (unsigned int i = 0; i < links_all_branch.size(); ++i)
{
std::vector<LineSkelethon> list_lines_one_chain;
for (unsigned int j = 0; j < links_all_branch[i].size() -1 ; ++j)
{
try
{
tf_listener.waitForTransform(base, links_all_branch[i][j], ros::Time(0), ros::Duration(2.0));
tf_listener.lookupTransform( base , links_all_branch[i][j], ros::Time(0), trans);
tf_listener.waitForTransform(base, links_all_branch[i][j+1], ros::Time(0), ros::Duration(2.0));
tf_listener.lookupTransform( base , links_all_branch[i][j+1], ros::Time(0), trans2);
tf::Vector3 trasnX = trans.getOrigin();
tf::Vector3 trasnX2 = trans2.getOrigin();
PointSkelethon Point1( LineCollisions::Point(trasnX.getX(), trasnX.getY(), trasnX.getZ()), links_all_branch[i][j] );
PointSkelethon Point2( LineCollisions::Point(trasnX2.getX(), trasnX2.getY(), trasnX2.getZ()), links_all_branch[i][j] );
list_lines_one_chain.push_back( LineSkelethon( Point1, Point2) );
}
catch(tf::TransformException& ex)
{
ROS_ERROR("%s", ex.what());
}
}
List_lines_in_all_chains.push_back(list_lines_one_chain);
}
for (unsigned int i = 0; i < List_lines_in_all_chains.size() - 1; ++i)
{
for (unsigned int j = i+1; j < List_lines_in_all_chains.size() ; ++j)
{
for (unsigned int k = 0; k < List_lines_in_all_chains[i].size() ; ++k)
{
for (unsigned int l = 0; l < List_lines_in_all_chains[j].size(); ++l)
{
LineCollisions::Line L1(List_lines_in_all_chains[i][k].P1.P, List_lines_in_all_chains[i][k].P2.P);
LineCollisions::Line L2(List_lines_in_all_chains[j][l].P1.P, List_lines_in_all_chains[j][l].P2.P);
LineCollisions::Line collision_line = LineCollisionsLocal.getClosestPoints( L1, L2 );
if (collision_line.norm <= .15)
{
lines_and_collisions.push_back( LineSkelethon( PointSkelethon( collision_line.P1, List_lines_in_all_chains[i][k].P1.link_name ),
PointSkelethon( collision_line.P2, List_lines_in_all_chains[j][l].P1.link_name ) )) ;
ROS_INFO_STREAM("Collision in Point:" << lines_and_collisions.back().P1.P.transpose() << " of link " << lines_and_collisions.back().P1.link_name);
ROS_INFO_STREAM("Collision in Point:" << lines_and_collisions.back().P2.P.transpose() << " of link " << lines_and_collisions.back().P2.link_name);
}
}
}
}
}
// list_pub.publish(plot_lines(List_lines_in_all_chains, base, 1.0, 0.0, 0.0, 0, visualization_msgs::Marker::LINE_LIST));
point_pub.publish(plot_lines(List_lines_in_all_chains, base, 0.0, 1.0, 0.0, 1, visualization_msgs::Marker::POINTS));
std::vector<std::vector<LineSkelethon>> lis;
lis.push_back(lines_and_collisions);
collisions_lines_pub.publish(plot_lines(lis, base, 0.0, 0.0, 1.0, 2, visualization_msgs::Marker::LINE_LIST));
collisions_points_pub.publish(plot_lines(lis, base, 1.0, 0.0, 1.0, 3, visualization_msgs::Marker::POINTS));
ros::spinOnce();
rate.sleep();
}
return 0;
}
