int getNumDepthSubscribers()
{
    int n = realsense_points_pub.getNumSubscribers() + realsense_reg_points_pub.getNumSubscribers() + realsense_depth_image_pub.getNumSubscribers();
#ifdef V4L2_PIX_FMT_INZI
    n += realsense_infrared_image_pub.getNumSubscribers();
#endif
    return n;
}
Пример #2
0
void SBANode::publishTopics(/*const ros::TimerEvent& event*/)
{
  // Visualization
  if (cam_marker_pub.getNumSubscribers() > 0 || point_marker_pub.getNumSubscribers() > 0)
  { 
     drawGraph(sba, cam_marker_pub, point_marker_pub);
  }
}
Пример #3
0
void publishState(void)
{
	uint8_t buf[10];
	const int ret = libusb_control_transfer(dev, 0xC0, 0x32, 0x0, 0x0, buf, 10, 0);
	if (ret != 10)
	{
		ROS_ERROR_STREAM("Error in accelerometer reading, libusb_control_transfer returned " << ret);
		ros::shutdown();
	}
	
	const uint16_t ux = ((uint16_t)buf[2] << 8) | buf[3];
	const uint16_t uy = ((uint16_t)buf[4] << 8) | buf[5];
	const uint16_t uz = ((uint16_t)buf[6] << 8) | buf[7];
	
	const int16_t accelerometer_x = (int16_t)ux;
	const int16_t accelerometer_y = (int16_t)uy;
	const int16_t accelerometer_z = (int16_t)uz;
	const int8_t tilt_angle = (int8_t)buf[8];
	const uint8_t tilt_status = buf[9];
	
	// publish IMU
	sensor_msgs::Imu imu_msg;
	if (pub_imu.getNumSubscribers() > 0)
	{
		imu_msg.header.stamp = ros::Time::now();
		imu_msg.linear_acceleration.x = (double(accelerometer_x)/FREENECT_COUNTS_PER_G)*GRAVITY;
		imu_msg.linear_acceleration.y = (double(accelerometer_y)/FREENECT_COUNTS_PER_G)*GRAVITY;
		imu_msg.linear_acceleration.z = (double(accelerometer_z)/FREENECT_COUNTS_PER_G)*GRAVITY;
		imu_msg.linear_acceleration_covariance[0] = imu_msg.linear_acceleration_covariance[4]
			= imu_msg.linear_acceleration_covariance[8] = 0.01; // @todo - what should these be?
		imu_msg.angular_velocity_covariance[0] = -1; // indicates angular velocity not provided
		imu_msg.orientation_covariance[0] = -1; // indicates orientation not provided
		pub_imu.publish(imu_msg);
	}
	
	// publish tilt angle and status
	if (pub_tilt_angle.getNumSubscribers() > 0)
	{
		std_msgs::Float64 tilt_angle_msg;
		tilt_angle_msg.data = double(tilt_angle) / 2.;
		pub_tilt_angle.publish(tilt_angle_msg);
	}
	if (pub_tilt_status.getNumSubscribers() > 0)
	{
		std_msgs::UInt8 tilt_status_msg;
		tilt_status_msg.data = tilt_status;
		pub_tilt_status.publish(tilt_status_msg);
	}
}
  void updateCallback(const ros::TimerEvent& e)
  {
    static bool got_first = false;
    latest_dt = (e.current_real - e.last_real).toSec();
    latest_jitter = (e.current_real - e.current_expected).toSec();
    max_abs_jitter = max(max_abs_jitter, fabs(latest_jitter));
    Result::Enum the_result;
    bool was_new_frame = true;

    if ((not segment_data_enabled) //
        and (pose_pub.getNumSubscribers() > 0 or markers_pub.getNumSubscribers() > 0))
    {
      the_result = MyClient.EnableSegmentData().Result;
      if (the_result != Result::Success)
      {
        ROS_ERROR("Enable segment data call failed");
      }
      else
      {
        ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
        ROS_INFO("Segment data enabled");
        segment_data_enabled = true;
      }
    }

    if (segment_data_enabled)
    {
      while (was_new_frame and (the_result = MyClient.GetFrame().Result) == Result::Success)
        ;
      {
        now_time = ros::Time::now(); // try to grab as close to getting message as possible
        was_new_frame = process_frame();
        got_first = true;
      }

      if (the_result != Result::NoFrame)
      {
        ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
      }

      if (got_first)
      {
        max_period_between_updates = max(max_period_between_updates, latest_dt);
        last_callback_duration = e.profile.last_duration.toSec();
      }
    }
    diag_updater.update();
  }
 void publishObstacleMarker(const pcl::PointXYZ& obstacle_location)
 {
   if (marker_pub.getNumSubscribers())
   {
     visualization_msgs::Marker obstacle;
     obstacle.header.frame_id = nh.getNamespace() + "/kinect_depth";
     obstacle.header.stamp = ros::Time::now();
     obstacle.ns = "obstacle";
     obstacle.action = visualization_msgs::Marker::ADD;
     obstacle.pose.position.x = obstacle_location.x;
     obstacle.pose.position.y = obstacle_location.y;
     obstacle.pose.position.z = obstacle_location.z;
     obstacle.pose.orientation.w = 1.0;
     obstacle.id = 0;
     obstacle.type = visualization_msgs::Marker::SPHERE;
     obstacle.scale.x = 0.1;
     obstacle.scale.y = 0.1;
     obstacle.scale.z = 0.1;
     obstacle.color.r = 1.0;
     obstacle.color.g = 0.0;
     obstacle.color.b = 0.0;
     obstacle.color.a = 0.8;
     obstacle.lifetime = ros::Duration(1.0);
     marker_pub.publish(obstacle);
   }
 }
Пример #6
0
void callback(const ros::MessageEvent<variant_topic_tools::Message>&
              messageEvent) {
    boost::shared_ptr<const variant_topic_tools::Message> message =
        messageEvent.getConstMessage();
    boost::shared_ptr<const ros::M_string> connectionHeader =
        messageEvent.getConnectionHeaderPtr();

    if (!publisher) {
        bool latch = false;

        if (connectionHeader) {
            ros::M_string::const_iterator it = connectionHeader->find("latching");
            if ((it != connectionHeader->end()) && (it->second == "1"))
                latch = true;
        }

        ros::AdvertiseOptions options(publisherTopic, publisherQueueSize,
                                      message->getType().getMD5Sum(), message->getType().getDataType(),
                                      message->getType().getDefinition(), connectCallback);
        options.latch = latch;

        publisher = nodeHandle->advertise<variant_msgs::Variant>(publisherTopic,
                    publisherQueueSize, connectCallback, ros::SubscriberStatusCallback(),
                    ros::VoidConstPtr(), latch);
    }

    if(!lazy || publisher.getNumSubscribers()) {
        boost::shared_ptr<const variant_msgs::Variant> variantMessage =
            message->toVariantMessage();
        publisher.publish(variantMessage);
    }
    else
        subscriber = ros::Subscriber();
}
Пример #7
0
	LocationFileWriter(string filename, bool append) {
		marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 1000);
		// Wait for subscribers to register before placing markers.
		int n;
		ros::Rate r(10);
		while (ros::ok() && n < 50) {
			// Note: do not run "rostopic echo visualization_marker" or this will not work!
			if (marker_pub.getNumSubscribers() > 0) {
				break;
			}
			n++;
			r.sleep();
		}
		next_id = 0;
		if (append) {
			auto locs = parse_locations(filename);
			// Place markers and set next_id to the max id of the parsed locations.
			for (auto it = locs.begin(); it != locs.end(); it++) {
				placeMarker(*it);
				if (it->id > next_id) {
					next_id = it->id;
				}
			}
		}
		next_id++;
		outfile.open(filename, append ? ofstream::app : ofstream::trunc);
		loc_sub = nh.subscribe("clicked_point", 1, &LocationFileWriter::locHandler, this);
	}
Пример #8
0
int main(int argc, char** argv) {

  ros::init(argc, argv, "system_state");

  ros::NodeHandle handle;

  stats_pub = handle.advertise<igvc_msgs::system_stats>("system_stats", 10);

  double frequency = 10.0;

  if(handle.hasParam("frequency")) {
    handle.getParam("frequency", frequency);
  }

  ros::Rate rate(frequency);

  while(!ros::isShuttingDown()) {
    if(stats_pub.getNumSubscribers() > 0) {
      igvc_msgs::system_stats stats;
      stats.header.stamp = ros::Time::now();
      stats.memory = get_used_memory();
      stats.cpu = get_cpu_usage();
      stats_pub.publish(stats);
    }
    rate.sleep();
  }

  return 0;
}
Пример #9
0
void timerCallback(const ros::TimerEvent& event) {
  if(!initalized){ return; }



  if(supplyVoltagePub.getNumSubscribers() > 0 /*|| batteryStatePub.getNumSubscribers() > 0*/){
    std_msgs::Float32 voltsMsg = publishSupplyVoltage(mcphid);
    /*sensor_msgs::BatteryState battMsg;

    battMsg.header.stamp = ros::Time::now();
    battMsg.voltage = voltsMsg.data;
    battMsg.current = nanf();
    battMsg.charge = nanf();
    battMsg.capacity = nanf();
    battMsg.design_capacity = nanf();
    battMsg.percentage = nanf();

    batteryStatePub.publish(battMsg);*/
  }

  /*if(motorCurrentPub.getNumSubscribers() > 0){
    publishMotorCurrents(mcphid);
  }

  if(motorBackEMFPub.getNumSubscribers() > 0){
    publishBackEMF(mcphid);
  }*/
}
Пример #10
0
/**
 * @brief It publishes the boundaries of the planes periodically.
 * 
 * @param s Segmenter instance already initiliazated.
 * @param marPub Publisher.
 * @param frame_id Reference frame.
 */
void publishBoundaries(const ros::TimerEvent&, const MultiplePlaneSegmentation &s, const ros::Publisher &marPub, const std::string &frame_id) {

	if (marPub.getNumSubscribers() > 0) {
		std::vector<std::vector<pcl::PointXYZRGBA>> boundaries;
		s.getBoundaries(boundaries);
		
		for(int i = 0; i < boundaries.size(); i++) {
			std::vector< std::vector<double> > positions = std::vector< std::vector<double> >(boundaries[i].size() + 1, std::vector<double>(3,0));
			int j;

			if (boundaries[i].size() == 0) continue;

			// Lines between consecutive points.
			for(j = 0; j < boundaries[i].size(); j++) {
				pcl::PointXYZRGBA p = boundaries[i][j];

				positions[j][0] = p.x;
				positions[j][1] = p.y;
				positions[j][2] = p.z;
			}
			// create a line between last and first point.
			positions[j] = positions[0];

			double width = 0.03;
			std::vector<double> color;
			computeColor(i, boundaries.size(), color);
			double colorArr[] = {color[0], color[1], color[2], 1.0};
			marPub.publish(buildLineMarker(frame_id, i, positions, width, colorArr));
	 	}
	}
}
Пример #11
0
void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
{
  if (!g_advertised)
  {
    // If the input topic is latched, make the output topic latched, #3385.
    bool latch = false;
    ros::M_string::iterator it = msg->__connection_header->find("latching");
    if((it != msg->__connection_header->end()) && (it->second == "1"))
    {
      ROS_DEBUG("input topic is latched; latching output topic to match");
      latch = true;
    }
    g_pub = msg->advertise(*g_node, g_output_topic, 10, latch, conn_cb);
    g_advertised = true;
    ROS_INFO("advertised as %s\n", g_output_topic.c_str());
  }
  // If we're in lazy subscribe mode, and nobody's listening, 
  // then unsubscribe, #3389.
  if(g_lazy && !g_pub.getNumSubscribers())
  {
    ROS_DEBUG("lazy mode; unsubscribing");
    delete g_sub;
    g_sub = NULL;
  }
  else
    g_pub.publish(msg);
}
Пример #12
0
int main(int argc, char** argv) {

	ros::init(argc, argv, "add_noise_pcl");

	ROS_INFO("Starting kinect noise generator node...");

	ros::NodeHandle n;

	ros::param::param<bool>("~add_noise",add_noise,true);
	ros::param::param<double>("~noise_amount_coef",nac,1.0);

	ros::Subscriber sub;

	m_pub = n.advertise<sensor_msgs::PointCloud2> ("points_out", 1);

	ros::Rate r(1);

	ros::AsyncSpinner spinner(5);
	spinner.start();

	ROS_INFO("Spinning");

	while(ros::ok()) {

		if (m_pub.getNumSubscribers() != 0) sub = n.subscribe("points_in", 1, cloudCallback);
		else sub.shutdown();

		r.sleep();

	}

	spinner.stop();

}
Пример #13
0
	void handle_flow(const mavlink_message_t *msg) {
		if (flow_pub.getNumSubscribers() == 0)
			return;

		mavlink_optical_flow_t flow;
		mavlink_msg_optical_flow_decode(msg, &flow);

		mavros_extras::OpticalFlowPtr flow_msg =
			boost::make_shared<mavros_extras::OpticalFlow>();

		// Note: for ENU->NED conversion i swap x & y.
		flow_msg->header.stamp = ros::Time::now();
		flow_msg->flow_x = flow.flow_y;
		flow_msg->flow_y = flow.flow_x;
		flow_msg->flow_comp_m_x	 = flow.flow_comp_m_y;
		flow_msg->flow_comp_m_y	 = flow.flow_comp_m_x;
		flow_msg->quality = flow.quality;
		flow_msg->ground_distance = flow.ground_distance;

		flow_pub.publish(flow_msg);

		/* Optional TODO: send ground_distance in sensor_msgs/Range
		 *                with data filled by spec on used sonar.
		 */
	}
Пример #14
0
bool RosAriaNode::hasSonarSubscribers()
{
    int count = combined_range_pub.getNumSubscribers();
    for (int i = sonars__crossed_the_streams ? 8 : 0; i < 16; i++)
        count += range_pub[i].getNumSubscribers();
    return count > 0;
}
Пример #15
0
	void placeMarker(snacbot::Location l) {
		visualization_msgs::Marker m;
		m.header.frame_id = "map";
		m.header.stamp = ros::Time::now();
		m.ns = "fast_smart_good";
		m.id = l.id;
		m.frame_locked = true;
		m.type = visualization_msgs::Marker::SPHERE;
		m.action = visualization_msgs::Marker::ADD;
		m.pose.position.x = l.x;
		m.pose.position.y = l.y;
		m.pose.position.z = 1;
		m.pose.orientation.x = 0.0;
		m.pose.orientation.y = 0.0;
		m.pose.orientation.z = 0.0;
		m.pose.orientation.w = 1.0;
		m.scale.x = 0.2;
		m.scale.y = 0.2;
		m.scale.z = 0.2;
		m.color.a = 1.0;
		m.color.r = 0.8;
		m.color.g = 0.2;
		m.color.b = 0.1;
		m.text = to_string(l.id);
		marker_pub.publish(m);
		ROS_INFO("published loc %ld at (%.2f, %.2f)", l.id, l.x, l.y);
		ROS_INFO("marker info %d", marker_pub.getNumSubscribers());
	}
Пример #16
0
/// Called when another node subscribes or unsubscribes from sonar topic.
void RosAriaNode::sonarConnectCb()
{
  publish_sonar = (sonar_pub.getNumSubscribers() > 0);
  publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0);
  robot->lock();
  if (publish_sonar || publish_sonar_pointcloud2)
  {
    robot->enableSonar();
    sonar_enabled = false;
  }
  else if(!publish_sonar && !publish_sonar_pointcloud2)
  {
    robot->disableSonar();
    sonar_enabled = true;
  }
  robot->unlock();
}
Пример #17
0
// Connection callback that unsubscribes from the tracker if no one is subscribed.
void connectCallback(message_filters::Subscriber<CameraInfo> &sub_cam,
                     message_filters::Subscriber<GroundPlane> &sub_gp,
                     image_transport::SubscriberFilter &sub_col,
                     image_transport::SubscriberFilter &sub_dep,
                     image_transport::ImageTransport &it) {
    if(!pub_message.getNumSubscribers() && !pub_result_image.getNumSubscribers() && !pub_centres.getNumSubscribers() && !pub_detected_persons.getNumSubscribers()) {
        ROS_DEBUG("Upper Body Detector: No subscribers. Unsubscribing.");
        sub_cam.unsubscribe();
        sub_gp.unsubscribe();
        sub_col.unsubscribe();
        sub_dep.unsubscribe();
    } else {
        ROS_DEBUG("Upper Body Detector: New subscribers. Subscribing.");
        sub_cam.subscribe();
        sub_gp.subscribe();
        sub_col.subscribe(it,sub_col.getTopic().c_str(),1);
        sub_dep.subscribe(it,sub_dep.getTopic().c_str(),1);
    }
}
Пример #18
0
    void configureRviz()
    {
        marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
        ros::Rate loop_rate(10000);
        while(marker_pub.getNumSubscribers() < 1 )
        {
            loop_rate.sleep();
        }


    }
 int
 process(const ecto::tendrils& in, const ecto::tendrils& out)
 {
   int num_subscribers = pub_.getNumSubscribers();
   *has_subscribers_ = (num_subscribers != 0) ? true : false;
   // lazy publishing if appropriate conditions are met
   if(*in_ && (*has_subscribers_ || latched_)) { 
     pub_.publish(**in_);
   }
   return ecto::OK;
 }
Пример #20
0
void expanded_callback(const set<SearchLocation> &expanded)
{
    if(expanded_pub.getNumSubscribers() > 0)
    {
        pcl::PointCloud<pcl::PointXYZ> cloud;
        cloud.header.frame_id = "/map";
        for(auto location : expanded)
            cloud.points.push_back(pcl::PointXYZ(location.x,location.y,0));

        expanded_pub.publish(cloud);
    }
}
Пример #21
0
    void deleteMarker(turtlesim::Pose task_pose, int t_id)
    {
	visualization_msgs::Marker marker;
	// Set the frame ID and timestamp.  See the TF tutorials for information on these.
	marker.header.frame_id = "world";
	marker.header.stamp = ros::Time::now();

	// Set the namespace and id for this marker.  This serves to create a unique ID
	// Any marker sent with the same namespace and id will overwrite the old one
	marker.ns = "task_node";
	marker.id = t_id;

	// Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
	marker.type = visualization_msgs::Marker::CUBE;

	// Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
	marker.action = visualization_msgs::Marker::DELETE;

	// Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
	marker.pose.position.x = task_pose.x;
	marker.pose.position.y = task_pose.y;
	marker.pose.position.z = 0;
	marker.pose.orientation.x = 0.0;
	marker.pose.orientation.y = 0.0;
	marker.pose.orientation.z = task_pose.theta;
	marker.pose.orientation.w = 1.0;

	// Set the scale of the marker -- 1x1x1 here means 1m on a side
	marker.scale.x = 1.0;
	marker.scale.y = 1.0;
	marker.scale.z = 1.0;

	// Set the color -- be sure to set alpha to something non-zero!
	marker.color.r = 1.0f;
	marker.color.g = 1.0f;
	marker.color.b = 0.0f;
	marker.color.a = 1.0;

	marker.lifetime = ros::Duration();

	// Publish the marker
	while (marker_pub.getNumSubscribers() < 1)
	{
	  if (!ros::ok())
	  {
// 	    return 0;
	    break;
	  }
	  ROS_WARN_ONCE("Please create a subscriber to the marker");
	  sleep(1);
	}
	marker_pub.publish(marker);
    }
Пример #22
0
void messageCallback(const test_roscpp::TestArrayConstPtr& msg, ros::Publisher pub)
{
  test_roscpp::TestArray copy = *msg;
  copy.counter++;

  while (ros::ok() && pub.getNumSubscribers() == 0)
  {
    ros::Duration(0.01).sleep();
  }

  pub.publish(copy);
}
Пример #23
0
        /// Display stuff
        void publishStuff(bool all = true){


            if (pubMarker.getNumSubscribers()>0){
            // Publish Landmarks
                pubMarker.publish(odometry.getMapMarkers());
                pubMarker.publish(odometry.getMapObservations());

                // Publish Track (ie all path and poses estimated)
                pubMarker.publish(odometry.getTrackLineMarker());
            }

            if (pubTrack.getNumSubscribers()>0){
                pubTrack.publish(odometry.getTrackPoseMarker());
            }



            if (all){
                // Publish TF for each KF
                ROS_INFO("NOD = Publishing TF for each KF");
                const Frame::Ptrs& kfs = odometry.getKeyFrames();
                //const Frame::Ptr kf = kfs[0];
                for(uint i=0; i<kfs.size(); ++i){
                    pubTF.sendTransform(kfs[i]->getStampedTransform());
                }
            }

            // Publish TF for current frame
            const Frame::Ptr f = odometry.getLastFrame();
            if (f->poseEstimated()){
                pubTF.sendTransform(f->getStampedTransform());
                pubTF.sendTransform(f->getStampedTransform(true));
            }

        }
Пример #24
0
void RosAriaNode::sonarConnectCb()
{
  robot->lock();
  if (sonar_pub.getNumSubscribers() == 0)
  {
    robot->disableSonar();
    use_sonar = false;
  }
  else
  {
    robot->enableSonar();
    use_sonar = true;
  }
  robot->unlock();
}
Пример #25
0
void Mapper::setMap(DP* newPointCloud)
{
	// delete old map
	if (mapPointCloud)
		delete mapPointCloud;
	
	// set new map
	mapPointCloud = newPointCloud;
	icp.setMap(*mapPointCloud);
	
	// Publish map point cloud
	// FIXME this crash when used without descriptor
	if (mapPub.getNumSubscribers())
		mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, mapCreationTime));
}
Пример #26
0
void map_callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& msg)
{
  std::lock_guard<std::mutex> planning_lock(planning_mutex);
  if (!msg->points.empty())
  {
    while (current_index < msg->size())
    {
      search_problem.Octree->addPointToCloud(
          pcl::PointXYZ(msg->points[current_index].x, msg->points[current_index].y, 0), search_problem.Map);
      current_index++;
    }
    if (path_planner_map_pub.getNumSubscribers() > 0)
    {
      path_planner_map_pub.publish(search_problem.Map);
    }
  }
}
Пример #27
0
	void mavlink_pub_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
		MavlinkPtr rmsg(new Mavlink);

		if  (mavlink_pub.getNumSubscribers() == 0)
			return;

		rmsg->header.stamp = ros::Time::now();
		rmsg->len = mmsg->len;
		rmsg->seq = mmsg->seq;
		rmsg->sysid = mmsg->sysid;
		rmsg->compid = mmsg->compid;
		rmsg->msgid = mmsg->msgid;
		for (size_t i = 0; i < (mmsg->len + 7) / 8; i++)
			rmsg->payload64.push_back(mmsg->payload64[i]);

		mavlink_pub.publish(rmsg);
	}
Пример #28
0
void visionCloudDebug(
	TheiaCloudPtr cloudPtr,
	ros::Publisher & publisher
){
	if(!publisher.getNumSubscribers()) return;

	// create message from cloud
	sensor_msgs::PointCloud2 cloudMessage;
	toROSMsg(*cloudPtr, cloudMessage);

	/**
	* TODO
	*/
	cloudMessage.header.frame_id = "/camera_depth_optical_frame";

	publisher.publish(cloudMessage);
}
Пример #29
0
//MAIN
int main(int argc, char** argv)
{

	ros::init(argc, argv, "wheel_motor_node"); //Initialize node

	ros::NodeHandle n; //Create nodehandle object

	sub = n.subscribe("wheel_motor_rc", 1000, callBack); //Create object to subscribe to topic "wheel_motor_rc"
	
	pub = n.advertise<motor_controller::AdaCmd>("adaFruit",1000); //Create object to publish to topic "I2C"
	while(pub.getNumSubscribers()==0);//Prevents message from sending when publisher is not completely connected to subscriber.
	
	//ros::Rate loop_rate(10); //Set frequency of looping. 10 Hz
	
	setGPIOWrite(33,1); //Motor enable

	while(ros::ok())
	{
		//Update time
		current_time = ros::Time::now();
		//Check if interval has passed
		if(current_time - last_time > update_rate)
		{
			//Reset time
			last_time = current_time;
			if(sub.getNumPublishers() == 0) //In case of loss of connection to publisher, set controller outputs to 0
			{
				ROS_WARN("Loss of wheel motor controller input!");
				leftFrontWheel.setU(0); //Set controller inputs
				leftRearWheel.setU(0);
				rightRearWheel.setU(0);
				rightFrontWheel.setU(0);
			}

			controlFunction(); //Motor controller function
			pub.publish(generateMessage());
		}
		
			ros::spinOnce();
	}


	stopAllMotors();

	return 0;
}
void Mapper::setMap(DP* newPointCloud)
{
	// delete old map
	if (mapPointCloud)
		delete mapPointCloud;
	
	// set new map
	mapPointCloud = newPointCloud;
	cerr << "copying map to ICP" << endl;
  //FIXME: this is taking the all map instead of the small part we need
	icp.setMap(*mapPointCloud); // This do a full copy...
	
	
	cerr << "publishing map" << endl;
	// Publish map point cloud
	// FIXME this crash when used without descriptor
	if (mapPub.getNumSubscribers())
		mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*mapPointCloud, mapFrame, mapCreationTime));
}