Esempio n. 1
0
///////////////////////////////////////////////////////////////////////////////////////////////
// left mouse button handlers
void PixRasterBaseCtrl::LeftDown(Point p, dword keyflags)
{
	// translates mouse coordinate in page and page coordinates
	int page;
	Point pagePt;
	if(!PointToPage(p, page, pagePt))
	   return;
	   
	// found a page on point, scan it to get the marker on it (if any)

	// gets the PixRaster object
	PixBase *pixBase = pixRasterCtrl->GetPixBase();

	int minDist = ScaleToPage(5);
	Markers *markers = pixBase->GetMarkersEx(page);
	for(int i = 0; i < markers->GetCount(); i++)
	{
		Marker &marker = (*markers)[i];
		int index;
		Marker::HitKind hitKind = marker.Hit(pagePt, minDist, index);
		if(hitKind == Marker::Miss)
			continue;
		
		// found a marker on cursor -- start drag op
		dragPolygon = PointsToView(page, marker.DragOutline(pagePt, pagePt, minDist));
		selectedMarker = &marker;
		dragPoint = pagePt;
		dragPage = page;
	}
	
	
} // END PixRasterBaseCtrl::LeftDown()
Esempio n. 2
0
void Xml::loadMarkers(QVector<Markers> *markers)
{
    QDomDocument xml("xml");
    QFile file(fileName.append(".xml"));
    while (!file.open(QIODevice::ReadOnly))
    {
        QString path = QFileDialog::getOpenFileName(0, tr("Open file"), QDir::homePath(), tr("Markers files").append(" (*.xml)"));
        if (path.isEmpty()) // when file dialog was canceled
            return;
        file.setFileName(path);
    }
    if (!xml.setContent(&file))
    {
        file.close();
        return;
    }
    file.close();
    markers->clear(); // clear markers list after open XML file success

    // print out the element names of all elements that are direct children
    // of the outermost element.
    QDomElement docElem = xml.documentElement();
    Markers marker;
    for (QDomElement e=docElem.firstChildElement(); !e.isNull(); e=e.nextSiblingElement())
    {
        marker.setLabel(e.tagName());
        marker.setBeginOffset(e.attribute("beginOffset").toInt());
        marker.setEndOffset(e.attribute("endOffset").toInt());
        marker.setNote(e.attribute("note"));
        markers->append(marker);
    }
    qDebug() << "Xml::loadMarkers -- markers loaded";
}
Esempio n. 3
0
static void PageMarkers(Pix & source, PixRaster &pixRaster)
{
	Markers *markers;
	
	pixRaster.Add(source, true);
	pixRaster.Add(source, true);
	
	markers = pixRaster[0].GetMarkers();
	markers->Add(new Marker(Point(100, 100), Point(300, 400)));
	markers->Add(new Marker(Point(0, 500), Point(300, 800)));
	markers = pixRaster[1].GetMarkers();
	markers->Add(new Marker(Point(0, 0), Point(200, 100)));
	markers->Add(new Marker(Point(400, 500), Point(900, 600)));
	
}
int main(int argc, char** argv)
{ 
  marker.line_list(linelist);
  ros::init (argc, argv, "uncovered_regions");
  ros::NodeHandle nh;
  //publishers
  pub_waypoint = nh.advertise<pcl::PointCloud<pcl::PointXYZ>> ("waypoints", 1);
  pub_waypoint_marker = nh.advertise<visualization_msgs::Marker> ("all_nodes", 10);
  pub_mesh_marker= nh.advertise<visualization_msgs::Marker> ("mesh_marker", 10);
  pub_linelist = nh.advertise<visualization_msgs::Marker> ("line_list",10);
  pub_covered_regions=nh.advertise<visualization_msgs::Marker>("covered_regions",10);
  //subscribers
  ros::Subscriber odometry_sub = nh.subscribe<nav_msgs::Odometry>("quad_sim_sysid_node/pose", 1, GetOdom);
  //reading stl file
  Filereader file;
  string path=ros::package::getPath("koptplanner")+"/src/mesh.txt";
  file.read(path,mesh_coor);

  marker.triangle_list(mesh_marker);
  marker.triangle_list(covered_regions);
  std_msgs::ColorRGBA mesh_color;
  geometry_msgs::Point mesh_point;
  //define mesh color
  mesh_color.r = 0 ;
  mesh_color.g = 0.5 ;
  mesh_color.b = 0.5 ;
  mesh_color.a = 0.5;
  //assigning each triangle of mesh to triangle list marker
  for(int i=0;i<mesh_coor.size();i++)
  {

    mesh_point.x = mesh_coor[i][0];
    mesh_point.y = mesh_coor[i][1];
    mesh_point.z = mesh_coor[i][2];  
    mesh_marker.points.push_back(mesh_point);
    // marker.add_point(mesh_marker,mesh_point,mesh_color);
  }
  mesh_marker.color=mesh_color;
  path=ros::package::getPath("boiler_gazebo")+"/src/latestPath.csv";
  file.read(path,csv_values);

  // display results
  cout.precision(2);
  cout.setf(ios::fixed,ios::floatfield);
  path=ros::package::getPath("koptplanner")+"/src/tour.txt";
  file.read(path,tour);  


  // display results
  cout.precision(2);
  cout.setf(ios::fixed,ios::floatfield);

  //waypoint marker msg basically points
  marker.points(waypoint_marker_msg);
  //scaling down the size
  waypoint_marker_msg.scale.x = 0.1;
  waypoint_marker_msg.scale.y = 0.1;
  waypoint_marker_msg.scale.z = 0.1;

  std_msgs::ColorRGBA color_msg_local;
  waypoint_marker_msg.points.resize(csv_values.size());
  waypoint_marker_msg.colors.resize(csv_values.size());

  waypoint_msg.header.frame_id = "world"; //point cloud of positions
  waypoint_msg.height = 1; waypoint_msg.width =csv_values.size();
  for (int i=0;i<csv_values.size();i++)
  {
      for(int j=0;j<csv_values[i].size();j++)
      {
         cout<<csv_values[i][j]<<"  ";
      }
      cout<<endl;
      waypoint_msg.points.push_back(pcl::PointXYZ(csv_values[i][0],csv_values[i][1],csv_values[i][2]));
  }
  geometry_msgs::Point point_local;
  mesh_color.r=0.5;
  for(int i=0;i<csv_values.size();i++)
  { 
    int l=csv_values[i][6]-2;
    if(l>=0)
    {
      point_local.x=csv_values[i][0];point_local.y=csv_values[i][1];point_local.z=csv_values[i][2];
      marker.add_point(linelist,point_local,mesh_color);
     
      point_local.x=(mesh_marker.points.at(l*3).x+mesh_marker.points.at(l*3+1).x+mesh_marker.points.at(l*3+2).x)/3;
      point_local.y=(mesh_marker.points.at(l*3).y+mesh_marker.points.at(l*3+1).y+mesh_marker.points.at(l*3+2).y)/3;
      point_local.z=(mesh_marker.points.at(l*3).z+mesh_marker.points.at(l*3+1).z+mesh_marker.points.at(l*3+2).z)/3;
      marker.add_point(linelist,point_local,mesh_color);
    }
  }

  //dummy_msg because by erasing the waypoint we will not get correct id of covered triangle

  dummy_msg.header.frame_id = "world"; //point cloud of positions
  dummy_msg.height = 1; dummy_msg.width =csv_values.size();
  for (int i=0;i<csv_values.size();i++)
  {
    dummy_msg.points.push_back(pcl::PointXYZ(csv_values[i][0],csv_values[i][1],csv_values[i][2]));
  }
  ros::Rate loop_rate(10.0);
  while(ros::ok())
  {  
     pub_linelist.publish(linelist);
     ros::spinOnce();
     loop_rate.sleep();
  }
}
// int covered_no=0;
void GetOdom(const nav_msgs::Odometry::ConstPtr &msg) 
{
  nav_msgs::Odometry curr_pose = *msg;

  geometry_msgs::Point ps;
  ps.x = curr_pose.pose.pose.position.x;
  ps.y = curr_pose.pose.pose.position.y;
  ps.z = curr_pose.pose.pose.position.z;
  
  int i=0;
  while(i<(waypoint_msg.height*waypoint_msg.width))
  {
    float error=sqrt(pow((waypoint_msg.points.at(i).x-ps.x),2)+pow((waypoint_msg.at(i).y-ps.y),2)+ pow((waypoint_msg.at(i).z-ps.z),2));
    if (error<0.5)
    {
      waypoint_msg.erase(waypoint_msg.begin()+i);
    }
    i++;
  }
  i=0;
  while(i<(dummy_msg.height*dummy_msg.width))
  {
    float error=sqrt(pow((dummy_msg.points.at(i).x-ps.x),2)+pow((dummy_msg.at(i).y-ps.y),2)+ pow((dummy_msg.at(i).z-ps.z),2));
    if (error<0.5)
    {
      if (i==dummy_msg.width-1)
      {
        covered_ids.push_back(1);
        cout<<"id::"<<1<<endl;
      }
      else
      {
        covered_ids.push_back(csv_values[i][6]);
        cout<<"id::"<<csv_values[i][6]<<endl;
      }
    }
    i++;
  }
  
  cout<<"no of waypoints to be covered:"<<(waypoint_marker_msg.points.size()/2)<<endl;

  std_msgs::ColorRGBA color_msg_local,redcolor;
  geometry_msgs::Point point_local;
  waypoint_marker_msg.points.resize(waypoint_msg.height*waypoint_msg.width);
  waypoint_marker_msg.colors.resize(waypoint_msg.height*waypoint_msg.width);
  for (int i=0;i<(waypoint_msg.height*waypoint_msg.width);i++)
  {
      waypoint_marker_msg.points.at(i).x = waypoint_msg.points.at(i).x;       
      waypoint_marker_msg.points.at(i).y = waypoint_msg.points.at(i).y;      
      waypoint_marker_msg.points.at(i).z = waypoint_msg.points.at(i).z; 
      waypoint_marker_msg.colors.at(i).r=0;
      waypoint_marker_msg.colors.at(i).g=0;
      waypoint_marker_msg.colors.at(i).b=1;
      waypoint_marker_msg.colors.at(i).a=1;
  }

   //assigning each triangle of mesh to triangle list marker
  redcolor.r=1;redcolor.b=0;redcolor.g=0;redcolor.a=1;
  for(int i=0;i<covered_ids.size();i++)
  { int x=int(covered_ids.at(i)-2)*3;
    if(x>=0)
    { 
      marker.add_point(covered_regions,mesh_marker.points.at(x),redcolor);
      marker.add_point(covered_regions,mesh_marker.points.at(x+1),redcolor);
      marker.add_point(covered_regions,mesh_marker.points.at(x+2),redcolor);
    }
  }
  covered_ids.clear();

  pub_waypoint_marker.publish(waypoint_marker_msg);
  pub_waypoint.publish(waypoint_msg);
  pub_mesh_marker.publish(mesh_marker);
  pub_covered_regions.publish(covered_regions);

}
Esempio n. 6
0
void PixRasterBaseCtrl::MouseMove(Point p, dword keyflags)
{
	Point endDragPoint;
	int endDragPage;
	
	if(!pixRasterCtrl || !pixRasterCtrl->GetPixBase())
		return;
	
	// check what we're dragging....
	if(keyflags & K_MOUSEMIDDLE)
	{
		// moves with middle button pressed -- panning image
		
		// gets distance between current and pan point
		int dx = panPoint.x - p.x + panHScroll;
		int dy = panPoint.y - p.y + panVScroll;
	
		// gets max scrolling values	
		int hMax = hScrollBar.GetTotal();
		int vMax = vScrollBar.GetTotal();
		
		// sets new pan position
		if(dx < 0) dx = 0;
		if(dx > hMax) dx = hMax;
		if(dy < 0) dy = 0;
		if(dy > vMax) dy = vMax;
		hScrollBar.Set(dx);
		vScrollBar.Set(dy);
	
		Refresh();
	}
	else if((keyflags & K_MOUSELEFT) && selectedMarker)
	{
		// dragging a marker
		
		int minDist = ScaleToPage(5);
		PointToPage(p, endDragPage, endDragPoint);

		if(endDragPage == dragPage)
			dragPolygon = PointsToView(dragPage, selectedMarker->DragOutline(dragPoint, endDragPoint, minDist));
		else
			dragPolygon.Clear();
		pixRasterCtrl->Refresh();
	}
	else
	{
		// nothing else, just highlight marker under cursor
		// and set the appropriate cursor
		int page;
		Point pagePt;
		int minDist = ScaleToPage(5);
		Marker *newMarker = NULL;
		if(PointToPage(p, page, pagePt))
		{
			PixBase *pixBase = pixRasterCtrl->GetPixBase();
			Markers *markers = pixBase->GetMarkersEx(page);
			for(int iMarker = 0; iMarker < markers->GetCount(); iMarker++)
			{
				Marker &marker = (*markers)[iMarker];
				int index;
				if(marker.Hit(pagePt, minDist, index) != Marker::Miss)
				{
					newMarker = &marker;
					break;
				}
			}
		}
		if(highlightMarker != newMarker)
		{
			highlightMarker = newMarker;
			pixRasterCtrl->Refresh();
		}
	}
	
} // END PixRasterBaseCtrl::MouseMove()
/**
@brief Markers publication for the visualization of the calibration ressult on rviz
@param[in] clouds ball center acquisitions in all sensors
@param[in] lasers position of the sensors
@return vector<visualization_msgs::Marker>
*/
vector<visualization_msgs::Marker> createTargetMarkers(vector<pcl::PointCloud<pcl::PointXYZ> > clouds, vector<geometry_msgs::Pose> lasers, vector<string> deviceNames,
                                                       const vector<double>& RPY, const vector<double>& translation )
{
  tf::Transform t_rpy;
  tf::Quaternion q;

  if (translation.empty() && RPY.empty()) //user does not want to translate/rotate clouds and sensors.
  {
    t_rpy.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation is done
    q = tf::createQuaternionFromRPY(0.0, 0.0, 0.0 ); // no rotation
  	t_rpy.setRotation( q );
  }
  else if (translation.empty()) // only rotation given by the user, no translation
  {
    t_rpy.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation
    q = tf::createQuaternionFromRPY( RPY[0], RPY[1], RPY[2] ); // quaternion computation from given angles
  	t_rpy.setRotation( q );
  }
  else // rotation and translation given by the user
  {
  	t_rpy.setOrigin( tf::Vector3 (tfScalar(translation[0]), tfScalar(translation[1]), tfScalar(translation[2])) ); // translation given by the user
  	q = tf::createQuaternionFromRPY( RPY[0], RPY[1], RPY[2] ); // quaternion computation from given angles
  	t_rpy.setRotation( q );
  }

  static Markers marker_list;
	int nLasers=lasers.size();
	std::cout << "number of lasers: " << nLasers << std::endl;

	//Reduce the elements status, ADD to REMOVE and REMOVE to delete
	marker_list.decrement();

	// Create a colormap
	class_colormap colormap("hsv", nLasers, 1, false);

	visualization_msgs::Marker marker_centers;
	visualization_msgs::Marker marker_lasers[nLasers*5]; // *5 to make space for Y, Z axis and square that represents the laser

	marker_centers.header.frame_id = "/my_frame3";
	marker_centers.header.stamp = ros::Time::now();

	marker_centers.ns = "centers";
	marker_centers.id = 0;
	marker_centers.action = visualization_msgs::Marker::ADD;

	marker_centers.type = visualization_msgs::Marker::SPHERE_LIST;

	marker_centers.scale.x = 0.08;
	marker_centers.scale.y = 0.08;
	marker_centers.scale.z = 0.08;

  marker_centers.pose.orientation.x=q[0];
	marker_centers.pose.orientation.y=q[1];
	marker_centers.pose.orientation.z=q[2];
	marker_centers.pose.orientation.w=q[3];


	marker_lasers[0].color.a=1;
	marker_lasers[0].color.g=1;
	marker_lasers[1].color.b=1;
	marker_lasers[1].color.a=1;
	marker_lasers[2].color.r=1;
	marker_lasers[2].color.a=1;

	for(int n=0; n<clouds.size(); n++)
	{
		pcl::PointCloud<pcl::PointXYZ> cloud = clouds[n];
		std_msgs::ColorRGBA color;
		color = colormap.color(n);

		for ( int i = 0; i< cloud.points.size(); i++)
		{
			geometry_msgs::Point pt;
			pt.x= cloud.points[i].x;
			pt.y= cloud.points[i].y;
			pt.z= cloud.points[i].z;

			marker_centers.points.push_back(pt);
			marker_centers.colors.push_back(color);


		}         //end for
		marker_list.update(marker_centers);
	}

	//position and orientation of laser
	int counter = 0;
	for(int n=0; n<lasers.size(); n++)
	{
    std::cout << "laser "<< n << std::endl;
		geometry_msgs::Pose laser = lasers[n];
		tf::Transform t_laser;
		t_laser.setOrigin( tf::Vector3(laser.position.x, laser.position.y, laser.position.z) );
		tf::Quaternion q_laser(laser.orientation.x, laser.orientation.y, laser.orientation.z, laser.orientation.w);
		t_laser.setRotation(q_laser);
    std::cout << "laser transform" << std::endl;

		for (int k=0; k<5; k++)
		{
			marker_lasers[counter].header.frame_id = "/my_frame3";
			marker_lasers[counter].header.stamp = ros::Time::now();

			std::stringstream ss;
			ss << "Laser " << n << "Marker: " << k;

			marker_lasers[counter].ns = ss.str();
			marker_lasers[counter].action = visualization_msgs::Marker::ADD;

			std_msgs::ColorRGBA color;

			if (k<3)
			{
				marker_lasers[counter].type = visualization_msgs::Marker::ARROW;

				marker_lasers[counter].scale.x = 0.5;
				marker_lasers[counter].scale.y = 0.05;
				marker_lasers[counter].scale.z = 0.05;

				color.r = 0;
				color.g = 0;
				color.b = 0;
				color.a = 1;
			}
			else if (k == 3)
			{
				marker_lasers[counter].type = visualization_msgs::Marker::CUBE;
				marker_lasers[counter].scale.x = 0.15;
				marker_lasers[counter].scale.y = 0.15;
				marker_lasers[counter].scale.z = 0.15;

			}
			else
			{
				marker_lasers[counter].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
				marker_lasers[counter].text = deviceNames[n];
				marker_lasers[counter].scale.x = 0.15;
				marker_lasers[counter].scale.y = 0.15;
				marker_lasers[counter].scale.z = 0.15;
				std::cout << deviceNames[n] << std::endl;
			}

			tf::Transform t_aux;
			tf::Quaternion q_aux;
			t_aux.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation is done

			switch (k) {
			case 0:
			{
				q_aux = tf::createQuaternionFromRPY(0.0, 0.0, 0.0 ); // no rotation->this is the X axis
				color.r = 1.0;
				//std::cout << "2 " << k << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << color << std::endl;
				break;
			}
			case 1:
			{
				q_aux = tf::createQuaternionFromRPY( 0.0, 0.0, M_PI/2 ); // rotation to get Y axis from X
				color.g = 1.0;
				//std::cout << "2 " << k << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << color << std::endl;
				break;
			}
			case 2:
			{
				q_aux = tf::createQuaternionFromRPY( 0.0, -M_PI/2, 0.0); // rotation to get Z axis from X
				color.b = 1.0;
				//std::cout << "2 " << k << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << color << std::endl;
				break;
			}
			case 3:
			{
				q_aux = tf::createQuaternionFromRPY( 0.0, 0.0, 0.0); // no rotation->this is the square at the axes origin that represents the sensor
				color = colormap.color(n);
				//std::cout << "2 " << k << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << " " << color << std::endl;
				break;
			}
			default:
			{
				std::cout << "text" << std::endl;
				q_aux = tf::createQuaternionFromRPY( 0.0, 0.0, 0.0);  // no rotation->this is the square at the axes origin that represents the sensor
				color.r = 1.0;
				color.g = 1.0;
				color.b = 1.0;
				color.a = 1.0;
			}
			}

			t_aux.setRotation( q_aux );

			tf::Transform transform_laser = t_rpy * t_laser * t_aux;
			tf::Quaternion rotation_laser = transform_laser.getRotation();
			tf::Vector3 translation_laser = transform_laser.getOrigin();
      std::cout << "Transformation" << std::endl;
			std::cout << "translation " << translation_laser[0] << " " << translation_laser[1] << " " << translation_laser[2] << std::endl;
			std::cout << "rotation " << rotation_laser[0] << " " << rotation_laser[1] << " " << rotation_laser[2] << " " << rotation_laser[3] << std::endl;

			marker_lasers[counter].pose.position.y=translation_laser[1];
			if (k!=4)
			{
				marker_lasers[counter].pose.position.x=translation_laser[0];
				marker_lasers[counter].pose.position.z=translation_laser[2];
        std::cout << "k!4" << std::endl;
			}
			else
			{
				marker_lasers[counter].pose.position.x=translation_laser[0]+0.15;
				marker_lasers[counter].pose.position.z=translation_laser[2]+0.25;
        std::cout << "text" << std::endl;
			}

			marker_lasers[counter].pose.orientation.x=rotation_laser[0];
			marker_lasers[counter].pose.orientation.y=rotation_laser[1];
			marker_lasers[counter].pose.orientation.z=rotation_laser[2];
			marker_lasers[counter].pose.orientation.w=rotation_laser[3];
      std::cout << "pose" << std::endl;

			marker_lasers[counter].color=color;
      std::cout << "color" << std::endl;

			marker_list.update(marker_lasers[counter]);
      std::cout << "update" << std::endl;
			counter++;
		}
	}
  std::cout << "clean" << std::endl;
	//Remove markers that should not be transmitted
	marker_list.clean();
  std::cout << "finished" << std::endl;
	//Clean the marker_vector and put new markers in it;
	return marker_list.getOutgoingMarkers();

} //end function
/**
@brief Markers publication for the visualization of the calibration ressult on rviz
@param[in] clouds ball center acquisitions in all sensors
@param[in] lasers position of the sensors
@return vector<visualization_msgs::Marker>
*/
vector<visualization_msgs::Marker> createTargetMarkers(vector<pcl::PointCloud<pcl::PointXYZ> > clouds, vector<geometry_msgs::Pose> lasers,
                                                       const vector<double>& RPY, const vector<double>& translation )
{
  tf::Transform t_rpy;
  tf::Quaternion q;

  if (translation.empty() && RPY.empty()) //user does not want to translate/rotate clouds and sensors.
  {
    t_rpy.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation is done
    q = tf::createQuaternionFromRPY(0.0, 0.0, 0.0 ); // no rotation
  	t_rpy.setRotation( q );
  }
  else if (translation.empty()) // only rotation given by the user, no translation
  {
    t_rpy.setOrigin( tf::Vector3 (tfScalar(0), tfScalar(0), tfScalar(0)) ); // no translation
    q = tf::createQuaternionFromRPY( RPY[0], RPY[1], RPY[2] ); // quaternion computation from given angles
  	t_rpy.setRotation( q );
  }
  else // rotation and translation given by the user
  {
  	t_rpy.setOrigin( tf::Vector3 (tfScalar(translation[0]), tfScalar(translation[1]), tfScalar(translation[2])) ); // translation given by the user
  	q = tf::createQuaternionFromRPY( RPY[0], RPY[1], RPY[2] ); // quaternion computation from given angles
  	t_rpy.setRotation( q );
  }

	static Markers marker_list;
	int nLasers=lasers.size();

	//Reduce the elements status, ADD to REMOVE and REMOVE to delete
	marker_list.decrement();

	// Create a colormap
	class_colormap colormap("hsv",10, 1, false);

	visualization_msgs::Marker marker_centers;
	visualization_msgs::Marker marker_lasers[nLasers];

	marker_centers.header.frame_id = "/my_frame3";
	marker_centers.header.stamp = ros::Time::now();

	marker_centers.ns = "centers";
	marker_centers.action = visualization_msgs::Marker::ADD;

	marker_centers.type = visualization_msgs::Marker::SPHERE_LIST;

	marker_centers.scale.x = 0.08;
	marker_centers.scale.y = 0.08;
	marker_centers.scale.z = 0.08;

	marker_centers.pose.orientation.x=q[0];
	marker_centers.pose.orientation.y=q[1];
	marker_centers.pose.orientation.z=q[2];
	marker_centers.pose.orientation.w=q[3];

	/*marker_centers.color.b=1;
	   marker_centers.color.a=1;*/
	marker_lasers[0].color.a=1;
	marker_lasers[0].color.g=1;
	marker_lasers[1].color.b=1;
	marker_lasers[1].color.a=1;
	marker_lasers[2].color.r=1;
	marker_lasers[2].color.a=1;

	for(int n=0; n<clouds.size(); n++)
	{
		{
			pcl::PointCloud<pcl::PointXYZ> cloud = clouds[n];
			std_msgs::ColorRGBA color;
			color = colormap.color(n);

			for ( int i = 0; i< cloud.points.size(); i++)
			{
				geometry_msgs::Point pt;
				pt.x= cloud.points[i].x;
				pt.y= cloud.points[i].y;
				pt.z= cloud.points[i].z;

				marker_centers.points.push_back(pt);
				marker_centers.colors.push_back(color);


			} //end for
			marker_list.update(marker_centers);
		}
	}

	//position and orientation of laser
	for(int n=0; n<lasers.size(); n++)
	{
		{
			marker_lasers[n].header.frame_id = "/my_frame3";
			marker_lasers[n].header.stamp = ros::Time::now();

			std::stringstream ss;
			ss << "Laser " << n;

			marker_lasers[n].ns = ss.str();
			marker_lasers[n].action = visualization_msgs::Marker::ADD;

			marker_lasers[n].type = visualization_msgs::Marker::ARROW;

			marker_lasers[n].scale.x = 0.2;
			marker_lasers[n].scale.y = 0.1;
			marker_lasers[n].scale.z = 0.1;

			geometry_msgs::Pose laser = lasers[n];
			std_msgs::ColorRGBA color;

			color = colormap.color(n);

			tf::Transform t_laser;
			t_laser.setOrigin( tf::Vector3(laser.position.x, laser.position.y, laser.position.z) );
			tf::Quaternion q_laser(laser.orientation.x, laser.orientation.y, laser.orientation.z, laser.orientation.w);
			t_laser.setRotation(q_laser);

			t_laser= t_rpy * t_laser;
			q_laser = t_laser.getRotation();
			tf::Vector3 trans_laser = t_laser.getOrigin();

			marker_lasers[n].pose.position.x=trans_laser[0];
			marker_lasers[n].pose.position.y=trans_laser[1];
			marker_lasers[n].pose.position.z=trans_laser[2];

			marker_lasers[n].pose.orientation.x=q_laser[0];
			marker_lasers[n].pose.orientation.y=q_laser[1];
			marker_lasers[n].pose.orientation.z=q_laser[2];
			marker_lasers[n].pose.orientation.w=q_laser[3];

			marker_lasers[n].color=color;

			marker_list.update(marker_lasers[n]);
		}
	}

	//Remove markers that should not be transmitted
	marker_list.clean();

	//Clean the marker_vector and put new markers in it;
	return marker_list.getOutgoingMarkers();

} //end function