void PlotShapesData(SHAPE_DATA *shape, long shapes, double xmin, double xmax, double ymin, double ymax) {
long i, j;
long origPen;
origPen = set_linetype(0);
/*set_mapping(xmin, xmax, ymin, ymax);*/
for (i=0; i<shapes; i++) {
for (j=0; j<shape[i].nPages; j++) {
plot_lines(shape[i].xData[j], shape[i].yData[j], shape[i].nPoints[j],
PRESET_LINETYPE,0);
}
}
set_linetype(origPen);
}
int display_ruler(Tcl_Interp *interp,
GapIO *io,
CanvasPtr *canvas,
c_offset *contig_offset,
int *contig_array,
int num_contigs,
int disp_ruler,
int disp_ticks,
ruler_s *ruler,
char *frame,
PlotRec **ruler_coord)
{
char cmd[1024];
int i;
PlotRec *CArray;
int depth;
if (!disp_ruler) {
return 0;
}
if (NULL == (CArray = (PlotRec *)xmalloc(num_contigs * sizeof(PlotRec)))) {
return -1;
}
/* remove current ruler before drawing the new ruler */
Tcl_VarEval(interp, ruler->window, " delete contig", NULL);
Tcl_VarEval(interp, ruler->window, " delete tag", NULL);
Tcl_VarEval(interp, ruler->window, " delete tick", NULL);
/*
* set up CArray
*/
for (i = 0; i < num_contigs; i++) {
CArray[i].num = contig_array[i];
CArray[i].l.x1 = 1 + contig_offset[contig_array[i]].offset;
CArray[i].l.x2 = io_clength(io, contig_array[i])
+ contig_offset[contig_array[i]].offset;
CArray[i].colour = ruler->colour;
if (NULL == (CArray[i].type = (char *)xmalloc(40))) {
verror(ERR_FATAL, "display_ruler", "out of memory");
return -1;
}
sprintf(CArray[i].type, "{contig c_%d num_%d hl_%d S}",
i+1, contig_array[i], contig_array[i]);
strcpy(CArray[i].arrow, "none");
}
CalcYDepth(num_contigs, CArray, num_contigs, &depth);
for (i = 0; i < num_contigs; i++) {
CArray[i].l.y1 = CArray[i].l.y1 * ruler->offset;
CArray[i].l.y2 = CArray[i].l.y2 * ruler->offset;
}
#ifdef DEBUG
for (i = 0; i < num_contigs; i++) {
printf("%d %d %d %d %s\n", CArray[i].l.x1, CArray[i].l.y1,
CArray[i].l.x2, CArray[i].l.y2, CArray[i].colour);
}
#endif
plot_lines(interp, CArray, num_contigs, ruler->window, ruler->line_width);
*ruler_coord = CArray;
if (!disp_ticks) {
sprintf(cmd, "RulerWindowSize %d %s %s ", disp_ticks, frame,
ruler->window);
Tcl_Eval(interp, cmd);
return(0);
}
for (i = 0; i < num_contigs; i++) {
display_ruler_ticks(interp, canvas,
contig_offset[contig_array[i]].offset,
CArray[i].l.y1, ruler, 1, CArray[i].l.x2 -
CArray[i].l.x1 + 1);
}
sprintf(cmd, "RulerWindowSize %d %s %s ", 1, frame, ruler->window);
Tcl_Eval(interp, cmd);
return(0);
}
void draw_contours(double **fxy, double xmin, double xmax, double ymin, double ymax,
long nx, long ny, double *cval, long nc)
{
double x[2], y[2], f[2][2], xx[4], yy[4];
long ix, iy, ic;
double fxymax, fxymin, value;
double dx, dy;
long nn;
dx = (xmax-xmin)/(nx-1);
dy = (ymax-ymin)/(ny-1);
fxymin = fxymax = fxy[0][0];
for (ix=0; ix<nx; ix++) {
for (iy=0; iy<ny; iy++) {
if ((value = fxy[ix][iy])>fxymax)
fxymax = value;
if (value<fxymin)
fxymin = value;
}
}
/* eliminate meaningless contours */
for (ic=0; ic<nc; ic++) {
if (cval[ic]<fxymin) {
cval += 1;
nc--;
ic--;
}
else if (cval[ic]>fxymax) {
nc = ic;
break;
}
}
/* sweep over region of plot and draw contours */
for (ix=0; ix<nx-1; ix++) {
x[1] = (x[0] = ix+1) + 1;
for (iy=0; iy<ny-1; iy++) {
y[1] = (y[0] = iy+1) + 1;
f[0][0] = fxy[ix ][iy ];
f[0][1] = fxy[ix ][iy+1];
f[1][0] = fxy[ix+1][iy ];
f[1][1] = fxy[ix+1][iy+1];
for (ic=0; ic<nc; ic++) {
nn = 0;
if ((f[0][0]<=cval[ic] && f[1][0]>=cval[ic]) ||
(f[1][0]<=cval[ic] && f[0][0]>=cval[ic]) )
find_intersection(x[0],y[0],f[0][0],f[1][0],cval[ic],xx,yy,&nn);
if ((f[0][0]<cval[ic] && f[0][1]>=cval[ic]) ||
(f[0][1]<=cval[ic] && f[0][0]>cval[ic]) )
find_intersection(y[0],x[0],f[0][0],f[0][1],cval[ic],yy,xx,&nn);
if ((f[0][1]<=cval[ic] && f[1][1]>=cval[ic]) ||
(f[1][1]<=cval[ic] && f[0][1]>=cval[ic]) )
find_intersection(x[0],y[1],f[0][1],f[1][1],cval[ic],xx,yy,&nn);
if ((f[1][0]<=cval[ic] && f[1][1]>cval[ic]) ||
(f[1][1]<cval[ic] && f[1][0]>=cval[ic]) )
find_intersection(y[0],x[1],f[1][0],f[1][1],cval[ic],yy,xx,&nn);
while (nn>=2) {
nn -= 2;
xx[nn] = (xx[nn ]-1)*dx + xmin;
xx[nn+1] = (xx[nn+1]-1)*dx + xmin;
yy[nn] = (yy[nn ]-1)*dy + ymin;
yy[nn+1] = (yy[nn+1]-1)*dy + ymin;
plot_lines(xx+nn, yy+nn, 2, PRESET_LINETYPE,0);
}
}
}
}
}
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;
}
