/**
 * @brief TimeManipulator::toString
 * @param time
 * @return
 */
std::string TimeManipulator::str(const ros::Time& time)
{
  double tenths = time.toSec() - floor(time.toSec());
  long minutes_unix = MathManipulator::getUnsignedDivision((long) floor(time.toSec()), 60);
  int seconds = MathManipulator::getUnsignedRest((long) floor(time.toSec()), 60);
  long hours_unix = MathManipulator::getUnsignedDivision(minutes_unix, 60);
  int minutes = MathManipulator::getUnsignedRest(minutes_unix, 60);
  long days_unix = MathManipulator::getUnsignedDivision(hours_unix, 24);
  int hours = MathManipulator::getUnsignedRest(hours_unix, 24);
  long periods_of_400_years = MathManipulator::getUnsignedDivision(days_unix, 146097);
  int days_in_400_years_period = MathManipulator::getUnsignedRest(days_unix, 146097);
  if (days_in_400_years_period >= 32 * 1461 + 789)
  {
    days_in_400_years_period++;
  }
  if (days_in_400_years_period >= 57 * 1461 + 789)
  {
    days_in_400_years_period++;
  }
  if (days_in_400_years_period >= 82 * 1461 + 789)
  {
    days_in_400_years_period++;
  }
  int periods_of_4_years = days_in_400_years_period / 1461;
  int days_in_4_years_period = days_in_400_years_period % 1461;
  if (days_in_4_years_period >= 59)
  {
    days_in_4_years_period++;
  }
  if (days_in_4_years_period >= 425)
  {
    days_in_4_years_period++;
  }
  if (days_in_4_years_period >= 1157)
  {
    days_in_4_years_period++;
  }
  int year_in_4_years_period = days_in_4_years_period / 366;
  int year_days = days_in_4_years_period % 366;
  int year = year_in_4_years_period + periods_of_4_years * 4 + periods_of_400_years * 400 + 1970;
  int months_table[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  int month_counter = 0;
  while(year_days > months_table[month_counter])
  {
    year_days -= months_table[month_counter++];
  }
  int month = month_counter + 1;
  int day = year_days + 1;
  std::stringstream ss;
  ss << (month < 10 ? "0" : "") << month << (day < 10 ? "/0" : "/") << day << (year < 10 ? "/0" : "/") << year;
  ss << (hours < 10 ? " 0" : " ") << hours << (minutes < 10 ? ":0" : ":") << minutes;
  if (seconds + tenths > 0)
  {
    ss << (seconds < 10 ? ":0" : ":") << seconds + tenths;
  }
  return ss.str();
}
Exemplo n.º 2
0
void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
{
  cv::Mat img;
  try {
    img = cv_bridge::toCvShare(msg, "mono8")->image;
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
  }
  processUserActions();

  double diff1 = msg->header.stamp.toSec() - time_start.toSec();
//  std::cout<<"diff1: "<< diff1 <<std::endl;

  if((diff1 > time_window.toSec()) && state == SSTART){
      vo_->reset();
      vo_->start();
      state = SGOT_KEY_1;
  }
  vo_->addImage(img, msg->header.stamp.toSec());
  svo_scale_ = vo_->svo_scale_;

  if((vo_->stage() == FrameHandlerMono::STAGE_SECOND_FRAME) && state == SGOT_KEY_1)
  {    std::cout<<"the first frame at time: "<<msg->header.stamp.toSec()<<std::endl;
       time_first = msg->header.stamp;
       state = SGOT_KEY_2;
  }

  if((vo_->stage() == FrameHandlerMono::STAGE_DEFAULT_FRAME) && state == SGOT_KEY_2)
  {    //std::cout<<"the second frame at time: "<<msg->header.stamp.toSec()<<std::endl;
       time_second = msg->header.stamp;
       state = SGETTING_SCALE;
  }

  double diff2 = msg->header.stamp.toSec() - time_second.toSec();
//  std::cout<<"diff2: "<< diff2 <<std::endl;
//  std::cout<<"state: "<< state <<std::endl;

  if((diff2 > time_window.toSec()) && state == SGETTING_SCALE){
      std::cout<<"time_first: "<< time_first <<std::endl;
      std::cout<<"time_second: "<< time_second <<std::endl;
      if(VoNode::initCb()){
          state = SDEFAULT;
      }
  }

  visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());

  if(publish_markers_){
      visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map(), state == SDEFAULT, svo_scale_, our_scale_);
  }

  if(publish_dense_input_)
    visualizer_.exportToDense(vo_->lastFrame());

  if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
    usleep(100000);
}
Exemplo n.º 3
0
void printTime()
{
  ros::Duration d = current_time - start_time;

  if (paused)
    printf("\r [PAUSED]   Log Time: %13.6f   Duration: %.6f            \r", current_time.toSec(), d.toSec());
  else
    printf("\r [RUNNING]  Log Time: %13.6f   Duration: %.6f            \r", current_time.toSec(), d.toSec());
  fflush(stdout);
}
Exemplo n.º 4
0
void encoderTickCallback(const sek_drive::encoders::ConstPtr& msg)
{
    renc = - msg->right_wheel;
    lenc = msg->left_wheel;
    current_time = ros::Time::now();
    if ((current_time.toSec() - enc_loop_time.toSec())>=0.05)
    {
        calcOdom();
        enc_loop_time_2 = current_time;
    }
}
void currentCallback(const std_msgs::Float32::ConstPtr& current){
	last_time = curr_time;
	curr_time = ros::Time::now(); 
	if(last_time.toSec() != 0.0){
		inst_power = current->data*22.0;	
		float inst_coulombs = current->data*(curr_time.toSec() - last_time.toSec());
		//ROS_INFO("inst_coulombs = %f", inst_coulombs);
		//ROS_INFO("curr coulombs, before addition: %f", curr_coulombs);
		curr_coulombs = curr_coulombs + inst_coulombs;
		//ROS_INFO("curr_coulombs, after addition:  %f", curr_coulombs);
	} //else, ignore the first value
}
Exemplo n.º 6
0
  void callback(const tPointCloud::ConstPtr& rgb_cloud, 
                const tImage::ConstPtr& rgb_image, 
                const tCameraInfo::ConstPtr& rgb_caminfo,
                const tImage::ConstPtr& depth_image, 
                const tPointCloud::ConstPtr& cloud
                )
  {
    if (max_update_rate_ > 0.0)
    {
      NODELET_DEBUG("update set to %f", max_update_rate_);
      if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
      {
        NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
        return;
      }
    }
    else
      NODELET_DEBUG("update_rate unset continuing");

    last_update_ = ros::Time::now();

    rgb_cloud_pub_.publish(rgb_cloud);
    rgb_image_pub_.publish(rgb_image);
    rgb_caminfo_pub_.publish(rgb_caminfo);
    depth_image_pub_.publish(depth_image);
    cloud_pub_.publish(cloud);
  }
Exemplo n.º 7
0
int wait_for_iob_signal()
{
  if (iob_synchronized) {
    //std::cerr << "wait>" << std::endl;
    if (start_robothw) {
      // no wait
    }
    //std::cerr << "wait<" << std::endl;
    return 0;
  } else {
    //
    ros::Time rnow;
    ros::Duration tm = ros::Duration(0, g_period_ns);
    ros::WallDuration wtm = ros::WallDuration(0, 100000); // 0.1 ms
    while ((rnow = ros::Time::now()) < rg_ts) {
      wtm.sleep();
    }

    rg_ts += tm;
    if ((rg_ts - rnow).toSec() <= 0) {
      fprintf(stderr, "iob::overrun (%f[ms]), w:%f -> %f\n",
              (rnow - rg_ts).toSec()*1000, rnow.toSec(), rg_ts.toSec());
      do {
        rg_ts += tm;
      } while ((rg_ts - rnow).toSec() <= 0);
    }

    return 0;
  }

  return 0;
}
Exemplo n.º 8
0
Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
{
	// TF ready?
	Transform transform;
	try
	{
		if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
		{
			//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
			if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
			{
				ROS_WARN("odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!",
						fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_);
				return transform;
			}
		}

		tf::StampedTransform tmp;
		tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
		transform = rtabmap_ros::transformFromTF(tmp);
	}
	catch(tf::TransformException & ex)
	{
		ROS_WARN("%s",ex.what());
	}
	return transform;
}
Exemplo n.º 9
0
void BagFormat::writeData(const ros::Time& time, const QVector< Plot::LinkedBufferIterator* >& data)
{
	plot_msgs::Plot msg;
	msg.header.stamp = time;

	Q_FOREACH(Plot::LinkedBufferIterator* it, data)
	{
		if(!it)
			continue;

		const Plot::DataPoint& p = **it;
		const Plot* plot = it->plot();

		plot_msgs::PlotPoint point;
		point.name = plot->path().toStdString();
		point.value = p.value;
		msg.points.push_back(point);
	}

	try
	{
		m_bag.write("plot", time, msg);
	}
	catch(rosbag::BagException& e)
	{
		fprintf(stderr, "Bag exception: %s\n", e.what());
		fprintf(stderr, "Time was %lf\n", time.toSec());
		throw;
	}
}
double PositionCommand::chirp_command ()
{
    double max_freq = 5.0;
    double min_freq = 0.0;
    double max_amp  = 0.3;
    double min_amp  = 0.05;
    double duration = 15.0;
    double Freq, Amp, time, velocity ,f_slope , a_slope;
    ros ::Time curr_time = ros::Time::now();

    f_slope = (max_freq - min_freq  ) / duration;
    a_slope = (max_amp  - min_amp   ) / duration;


    time = curr_time.toSec() - time0_chirp_.toSec();

    Amp  = time * a_slope + min_amp;
    Freq = time * f_slope + min_freq;

    velocity = -Amp * sin ( 2 * PI * Freq * time);

    std::cout   << "f = "           << Freq
                << "\ttime = "       << time
                << "\tvelocity = "   << velocity
                << "\tAmp = "        << Amp
                << std::endl;

    if (Freq >= max_freq)
    {
        chirp_enable_ = false;
        velocity = 0;
    }
    return velocity;
}
Exemplo n.º 11
0
int NodeClass::requestBoardStatus() {
	int ret;	
	
	// Request Status of RelayBoard 
	ret = m_SerRelayBoard->sendRequest();
	if(ret != SerRelayBoard::NO_ERROR) {
		ROS_ERROR("Error in sending message to Relayboard over SerialIO, lost bytes during writing");
	}

	ret = m_SerRelayBoard->evalRxBuffer();
	if(ret==SerRelayBoard::NOT_INITIALIZED) {
		ROS_ERROR("Failed to read relayboard data over Serial, the device is not initialized");
		relayboard_online = false;
	} else if(ret==SerRelayBoard::NO_MESSAGES) {
		ROS_ERROR("For a long time, no messages from RelayBoard have been received, check com port!");
		if(time_last_message_received_.toSec() - ros::Time::now().toSec() > relayboard_timeout_) {relayboard_online = false;}
	} else if(ret==SerRelayBoard::TOO_LESS_BYTES_IN_QUEUE) {
		//ROS_ERROR("Relayboard: Too less bytes in queue");
	} else if(ret==SerRelayBoard::CHECKSUM_ERROR) {
		ROS_ERROR("A checksum error occurred while reading from relayboard data");
	} else if(ret==SerRelayBoard::NO_ERROR) {
		relayboard_online = true;
		relayboard_available = true;
		time_last_message_received_ = ros::Time::now();
	}

	return 0;
}
Exemplo n.º 12
0
	bool getPoseAt(const ros::Time& time, StateType& pose, ros::Time& stamp)
	{
		std::deque<TimeState> past_poses_copy;

		{
			boost::mutex::scoped_lock past_poses_lock(past_poses_mutex_);
			past_poses_copy = past_poses_;
		}

		if (past_poses_copy.empty())
			return false;

		TimeState result = *(past_poses_copy.rbegin());
		if (result.first <= time)
		{
			typename std::deque<TimeState>::const_reverse_iterator iter =
					past_poses_copy.rbegin()++;
			while(iter != past_poses_copy.rend() && iter->first <= time)
			{
				iter++;
			}
			if (iter != past_poses_copy.rend())
			{
				// check which time is the nearest
				double delta_after = fabs(iter->first.toSec() - time.toSec());
				double delta_before = fabs(
						(iter - 1)->first.toSec() - time.toSec());
				if (delta_before < delta_after)
				{
					result = *(--iter);
				}
				else
				{
					result = *iter;
				}
			}
			else
			{
				result = *(--iter);
			}
		}

		pose = result.second;
		stamp = result.first;
		return true;
	}
Exemplo n.º 13
0
void TaskScheduler::enqueueAction(const ros::Time & when,  ActionType type,boost::shared_ptr<ThreadParameters> tp)
{
	ThreadAction ta;
	PRINTF(3,"ea:Locking\n");
	pthread_mutex_lock(&aqMutex);
	PRINTF(3,"ea:Locked\n");
	// if (!runScheduler) return;
	PRINTF(2,"Enqueing action %.3f %s -- %s\n",when.toSec(),actionString(type),
			tp?(tp->task->getName().c_str()):"none");

	ta.type = type;
	ta.tp = tp;
	actionQueue[when.toSec()] = ta;
	PRINTF(3,"ea:Signalling\n");
	pthread_cond_signal(&aqCond);
	PRINTF(3,"ea:Unlocking\n");
	pthread_mutex_unlock(&aqMutex);
}
Exemplo n.º 14
0
  void imageCb_rgb(const sensor_msgs::ImageConstPtr& msg)
  {

     sensor_msgs::CvBridge bridge;//we need this object bridge for facilitating conversion from ros-img to opencv
     IplImage* img = bridge.imgMsgToCv(msg,"bgr8");  //image being converted from ros to opencv using cvbridge

     if (REC==1)
     {
         rgb_cur_time = ros::Time::now();
         cvShowImage( "RGB8 IMG",img);
         if(((rgb_cur_time.toSec())-(rgb_last_time.toSec()))>0.1)
         {
             time(&t);
             timer = (long)t;
             sstream << timer;
             path_ts = sstream.str();
             path_ts=path+path_ts+png;
             ros::Duration(0.01).sleep();
              printf("path %s\n",path_ts.c_str());
              const char* cstr = path_ts.c_str();
             // printf("lolol %s\n",cstr);
             //if(!(cvSaveImage(path_ts.c_str(),img)))
             if(!(cvSaveImage(cstr,img)))
             {
              printf("Can't Capture RGB Image\n");
              ros::Duration(0.01).sleep();
             }
             else
             {
                printf("RGB Image Captured\n");
                ros::Duration(0.01).sleep();
             }
             rgb_last_time=rgb_cur_time;
             rgb_cur_time=ros::Time::now();
             path_ts="";
             sstream.str(std::string());
             sstream.clear();
             //printf("path %s\n",path_ts.c_str());
          
         }

     }
      cvWaitKey(2);
    }
  void scannerSignalCallback(const sensor_msgs::JointStateConstPtr& js)
  {

    double ang = fmod(js->position[0], 2 * M_PI);
    // ROS_DEBUG("ang = %lf, prev_angle = %lf, %lf", ang, prev_angle_, prev_signal_.toSec());

    if ( prev_angle_ < 0 ) {
      prev_angle_ = ang;
      return;
    }
    if ((ang - prev_angle_) >= - M_PI) {
      prev_angle_ = ang;
      return;
    }

    if (prev_signal_.toSec() == 0.0) {
      first_time_ = true;
    }

    ros::Time stmp = js->header.stamp;
    if (first_time_)
    {
      prev_signal_ = stmp;
      first_time_ = false;
    }
    else
    {
      if (num_skips_ > 0)
      {
        if (num_skips_left_ > 0)
        {
          num_skips_left_ -= 1;
          return;
        }
        else
        {
          num_skips_left_ = num_skips_;
        }
      }

      laser_assembler::AssembleScans2::Request req;
      laser_assembler::AssembleScans2::Response res;

      req.begin = prev_signal_;
      req.end   = stmp;

      if (!ros::service::call("assemble_scans2", req, res))
        ROS_ERROR("Failed to call service on point cloud assembler or laser scan assembler.");

      pub_.publish(res.cloud);
      ROS_INFO("Snapshotter::Published Cloud size=%u", res.cloud.width * res.cloud.height);

      prev_signal_ = stmp;
      prev_angle_ = -1;
    }
  }
Exemplo n.º 16
0
void OdometryFromPose::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
  nav_msgs::Odometry odom;
  tf::Quaternion quat;
  geometry_msgs::Vector3 rpy;
  ros::Time current_time;

  double roll, pitch, yaw, dt;


  // transform quaternion to fixed axis angles

  tf::quaternionMsgToTF(msg->pose.pose.orientation, quat);

  // First we have to turn this quaternion to a rotation matrix and then use the accessor getRPY on this matrix.
  tf::Matrix3x3 m(quat);
  m.getRPY(roll, pitch, yaw);

  // Store and publih the angles
  rpy.x = roll;
  rpy.y = pitch;
  rpy.z = yaw;
  rpy_pub_.publish(rpy);

  //set the position to odometry msg
  odom.header.stamp = msg->header.stamp;
  odom.header.frame_id = "map";
  odom.pose.pose = msg->pose.pose;


  // calculate veocity by calcualting derivatives
  current_time = ros::Time::now();
  dt = current_time.toSec() - last_time_.toSec();

  //odom.child_frame_id = "base_link";
  odom.twist.twist.linear.x = (msg->pose.pose.position.x - previous_x_) / dt;
  odom.twist.twist.linear.y = (msg->pose.pose.position.y - previous_y_) / dt;
  odom.twist.twist.linear.z = (msg->pose.pose.position.z - previous_z_) / dt;

  //curr_yaw = tf::getYaw(msg->pose.pose.orientation);
  odom.twist.twist.angular.z = (yaw - previous_yaw_)/ dt;



  odom_pub_.publish(odom); // publish the complete odometry msg


  // Store the current values as previous for the next loop
  last_time_ = current_time;
  previous_x_ = msg->pose.pose.position.x;
  previous_y_ = msg->pose.pose.position.y;
  previous_z_ = msg->pose.pose.position.z;
  previous_yaw_ = yaw;

}
Exemplo n.º 17
0
  void imageCb_di(const sensor_msgs::ImageConstPtr& msg)
    {
	
        sensor_msgs::CvBridge depth_bridge;
        IplImage* img_dd = depth_bridge.imgMsgToCv(msg,"mono16");
        cvShowImage( "pew",img_dd);
        if (REC==1)
             {
                depth_cur_time = ros::Time::now();
                if(((depth_cur_time.toSec())-(depth_last_time.toSec()))>0.1)
                {	
				
                    time(&t);
                    timer = (long)t;
                    sstream << timer;
                    path2_ts = sstream.str();
                    path2_ts=path2+path2_ts+png;
                    //printf("path 2 %s\n",path2_ts.c_str());
                    const char* cstr2 = path2_ts.c_str();
                    //ros::Duration(2).sleep();
                    if(!(cvSaveImage(cstr2,img_dd)))
                    {
                          printf("Can't Capture Depth Image\n");
                          //ros::Duration(2).sleep();
                    }
                    else
                    {
                         //printf("Depth Image Captured\n");
                          //ros::Duration(2).sleep();
                    }
                    path2_ts="";
                    sstream.str(std::string());
					sstream.clear();
                    //printf("path 2 cleared %s\n",path2_ts.c_str());
                    depth_last_time = depth_cur_time;
                    depth_cur_time=ros::Time::now();
                }
             }
        //cvShowImage( "pew",img_dd);
        cvWaitKey(2);
    
}
    void update(common_odom_estimation_data &data, common_odom_estimation_config config)
    {
        /* protected region user update on begin */

        if(localconfig.enable_fake && localconfig.enable_beacon && localconfig.enable_imu)
        {
		//if(last_beacon_time.toSec() > 0.1)
		//{
			if(fabs(last_cmd_vel.linear.x) > 0.01 || fabs(last_cmd_vel.linear.y) > 0.01)
			{
				double dt = ros::Time::now().toSec() - data.out_odom_est.header.stamp.toSec();
				//estimated_pose.x += ( (integral_imu_x * cos(estimated_pose.theta) + integral_imu_y * sin(estimated_pose.theta)) * dt);
				//estimated_pose.y += ( (integral_imu_y * cos(estimated_pose.theta) + integral_imu_x * sin(estimated_pose.theta)) * dt);
                                estimated_pose.x += ( (last_cmd_vel.linear.x * cos(estimated_pose.theta) - last_cmd_vel.linear.y * sin(estimated_pose.theta)) * dt);
                                estimated_pose.y += ( (last_cmd_vel.linear.y * cos(estimated_pose.theta) + last_cmd_vel.linear.x * sin(estimated_pose.theta)) * dt);
			}
		//}
	}

	if(localconfig.enable_fake && localconfig.enable_beacon && !localconfig.enable_imu)
        {
        	double dt = ros::Time::now().toSec() - data.out_odom_est.header.stamp.toSec();
	        if(last_beacon_time.toSec() > 0.1) 
                {       
                        if(fabs(last_cmd_vel.linear.x) > 0.01 || fabs(last_cmd_vel.linear.y) > 0.01 )
                        {       
                                estimated_pose.x += ( (last_cmd_vel.linear.x * cos(estimated_pose.theta) + last_cmd_vel.linear.y * sin(estimated_pose.theta)) * dt);
                                estimated_pose.y += ( (last_cmd_vel.linear.y * cos(estimated_pose.theta) + last_cmd_vel.linear.x * sin(estimated_pose.theta)) * dt);
                        }       
                }
		if(fabs(last_cmd_vel.angular.z) > 0.001 )
                {
			estimated_pose.theta += ( last_cmd_vel.angular.z * dt );
		}

        }

	t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(estimated_pose.theta), tf::Vector3(estimated_pose.x, estimated_pose.y, 0.0)),
		                                ros::Time::now(), localconfig.parent_link, localconfig.child_link);

	t.stamp_ = ros::Time::now();
	broadcaster.sendTransform(t);

	data.out_odom_est.child_frame_id = localconfig.child_link;
	data.out_odom_est.header.frame_id = localconfig.parent_link;
	data.out_odom_est.header.stamp = ros::Time::now();

	data.out_odom_est.pose.pose.position.x = estimated_pose.x;
	data.out_odom_est.pose.pose.position.y = estimated_pose.y;
	data.out_odom_est.pose.pose.position.z = 0.0;

	data.out_odom_est.pose.pose.orientation = tf::createQuaternionMsgFromYaw(estimated_pose.theta);
        /* protected region user update end */
    }
void EasyLight::execute(void)
{
	if(enableOff.data == true)
        {
                if( (ros::Time::now().toSec() - lastOff.toSec()) > 10.0)
		{
			std_msgs::Empty empty;
                	off_pub.publish(empty);
                	enableOff.data = false;
		}
        }
}
Exemplo n.º 20
0
void ResultFileWriter::writePoseData(const ros::Time &time_stamp, const PoseData &data) const
{
  ofstream file;
  file.open(file_name_.c_str(), ios::app);
  file.setf(ios::fixed, ios::floatfield);
  file << time_stamp.toSec() << ','
       << data.position.x << ',' << data.position.y << ',' << data.position.z << ','
       << data.orientation.x << ',' << data.orientation.y << ',' << data.orientation.z << ','
       << data.velocity.linear.x << ',' << data.velocity.linear.y << ',' << data.velocity.linear.z << ','
       << data.velocity.angular.x << ',' << data.velocity.angular.y << ',' << data.velocity.angular.z <<'\n';
  file.close();
}
Exemplo n.º 21
0
vector<classification::DataPoint*> classification::DataLoader::getDataSubset(
		vector<DataPoint*> &data, ros::Time start, ros::Time end)
{
	vector<DataPoint*> subset;

	// Times are not totally accurate, floor them to round to closest second
	double startTime = floor(start.toSec());
	double endTime = floor(end.toSec());

	// Find and push all DataPoints between start and end times
	BOOST_FOREACH(DataPoint *point, data){
		double time = floor(point->getTimestamp().toSec());

		// Add 0.3 seconds to compensate for inaccuracies
		if (time >= startTime && time + 0.3 <= endTime) {
			subset.push_back(point);
		}
		else if (time + 0.3 > endTime) {
			break;
		}
	}
Exemplo n.º 22
0
ardrone_autonomy::Navdata ObjectDetector::navdataInterpolate(const ardrone_autonomy::Navdata & nd0, ardrone_autonomy::Navdata & nd1, ros::Time & middle_stamp)
{
    double  d0 = 1-(middle_stamp.toSec() - nd0.header.stamp.toSec()) / (nd1.header.stamp.toSec() - nd0.header.stamp.toSec()),
            d1 = 1-(nd1.header.stamp.toSec() - middle_stamp.toSec()) / (nd1.header.stamp.toSec() - nd0.header.stamp.toSec());

    ardrone_autonomy::Navdata _navdata;
    #define navdata_mean(var) _navdata.var = (nd0.var*d0 + nd1.var*d1)
    navdata_mean(rotX);
    navdata_mean(rotY);
    navdata_mean(rotZ);
    navdata_mean(vx);
    navdata_mean(vy);
    navdata_mean(vz);
    navdata_mean(ax);
    navdata_mean(ay);
    navdata_mean(az);
    navdata_mean(altd);
    navdata_mean(pressure);
    #undef navdata_mean
    return _navdata;
}
Exemplo n.º 23
0
		/********** callback for the cmd velocity from the autonomy **********/
		void cmd_vel_callback(const geometry_msgs::Twist& msg)
		{
			watchdogTimer.stop();
			
			error.setValue(msg.linear.x - body_vel.linear.x, msg.linear.y - body_vel.linear.y, msg.linear.z - body_vel.linear.z);
			//std::cout << "error x: " << error.getX() << " y: " << error.getY() << " z: " << error.getZ() << std::endl;
			//std::cout << std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) << std::endl;
			error_yaw = msg.angular.z - body_vel.angular.z;
			//std::cout << "error yaw: " << error_yaw << std::endl;
			
			// if some time has passed between the last body velocity time and the current body velocity time then will calculate the (feed forward PD)
			if (std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) > 0.00001)
			{	
				errorDot = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error - last_error);
				//std::cout << "errordot x: " << errorDot.getX() << " y: " << errorDot.getY() << " z: " << errorDot.getZ() << std::endl;
				
				errorDot_yaw = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error_yaw - last_error_yaw);
				//std::cout << "error dot yaw " << errorDot_yaw << std::endl;
				velocity_command.linear.x = cap_vel_auton(kx*msg.linear.x + (kp*error).getX() + (kd*errorDot).getX());
				velocity_command.linear.y = cap_vel_auton(ky*msg.linear.y + (kp*error).getY() + (kd*errorDot).getY());
				velocity_command.linear.z = cap_vel_auton(kz*msg.linear.z + (kp*error).getZ() + (kd*errorDot).getZ());
				velocity_command.angular.z = -1*cap_vel_auton(kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw); // z must be switched because bebop driver http://bebop-autonomy.readthedocs.org/en/latest/piloting.html
			}
			
			last_body_vel_time = curr_body_vel_time;// update last time body velocity was recieved
			last_error = error;
			last_error_yaw = error_yaw;
			
			error_gm.linear.x = error.getX(); error_gm.linear.y = error.getY(); error_gm.linear.z = error.getZ(); error_gm.angular.z = error_yaw;
			errorDot_gm.linear.x = errorDot.getX(); errorDot_gm.linear.y = errorDot.getY(); errorDot_gm.linear.z = errorDot.getZ(); errorDot_gm.angular.z = kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw;
			error_pub.publish(error_gm);
			errorDot_pub.publish(errorDot_gm);
			
			if (start_autonomous)
			{
				recieved_command_from_tracking = true;
			}
			
			watchdogTimer.start();
		}
QString VideoEditorWidget::timeFromMsg(const ros::Time& stamp)
{
    int sec = stamp.toSec();
    std::stringstream stream;

    stream.str("");
    int day = sec/86400;
    sec -= day * 86400;

    int hour = sec / 3600;
    sec -= hour * 3600;

    int min = sec / 60;
    sec -= min * 60;
    uint32_t nano = (stamp.toSec() - (int)stamp.toSec())*1000;
    stream << std::setw(2) << std::setfill('0') << day << " ";
    stream << std::setw(2) << std::setfill('0') << hour << ":";
    stream << std::setw(2) << std::setfill('0') << min << ":";
    stream << std::setw(2) << std::setfill('0') << sec << ".";
    stream << std::setw(3) << std::setfill('0') << nano;
    return QString::fromStdString(stream.str());
}
void PositionCommand::getPose(const geometry_msgs::PoseStamped::ConstPtr & Pose)
{
    rot_matrix_= Eigen::Quaterniond(Pose->pose.orientation.w,
                                    Pose->pose.orientation.x,
                                    Pose->pose.orientation.y,
                                    Pose->pose.orientation.z).matrix();

    Eigen::Matrix<double,3,1> euler = rot_matrix_.eulerAngles(2, 1, 0);

    double yaw   = euler(0,0);
    double pitch = euler(1,0);
    double roll  = euler(2,0);

    current_pose_ <<    Pose->pose.position.x,
            Pose->pose.position.y,
            Pose->pose.position.z,
            roll,
            pitch,
            yaw;

    // if it is the first time: initialize the function
    if(init_)
    {
        ROS_INFO("initialize");
        previous_time_  = ros::Time::now();
        goal_pose_      << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
        init_           = false;
        control_        = false;
        chirp_enable_   = false;
    }

    ros::Time current_time  = Pose->header.stamp;
    period_                 = current_time.toSec() - previous_time_.toSec();
    previous_time_          = current_time;

    control_ = true;

    control_info_msg.Period         = period_;
    control_info_msg.header.stamp   = current_time;
    control_info_pub.publish(control_info_msg);


    //    std::cout << "CP: x = " <<  current_pose_(0,0)
    //              << " y = "    <<  current_pose_(1,0)
    //              << " z = "    <<  current_pose_(2,0)
    //              << " w = "    <<  current_pose_(5,0)
    //              << std::endl;
    //    std::cout << "Period ="  <<  period_ << std::endl;
}
Exemplo n.º 26
0
void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
  veltime = ros::Time::now();
#ifdef SPEW
  ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
#endif

  robot->lock();
  robot->setVel(msg->linear.x*1e3);
  robot->setRotVel(msg->angular.z*180/M_PI);
  robot->unlock();
  ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
    (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}
void ubicacionCuerpoCallback(geometry_msgs::PoseStamped msgUbicacionCuerpo)
{
    tiempo_anterior2 = tiempo_ahora2;
    time_stamp = msgUbicacionCuerpo.header.stamp;
    tiempo_ahora2 = time_stamp.toSec();
    ubicacionRobot.coordenadaCuerpo_x = msgUbicacionCuerpo.pose.position.x;
    ubicacionRobot.coordenadaCuerpo_y = msgUbicacionCuerpo.pose.position.y;
    tf::quaternionMsgToTF(msgUbicacionCuerpo.pose.orientation,CuerpoOrientacion_Q);
    tf::Matrix3x3(CuerpoOrientacion_Q).getRPY(roll, pitch, yaw);
    ubicacionRobot.orientacionCuerpo_roll = roll;
    ubicacionRobot.orientacionCuerpo_pitch = pitch;
    ubicacionRobot.orientacionCuerpo_yaw = yaw;

    infoCuerpo = true;
}
Exemplo n.º 28
0
string time_string(const ros::Time &rtime) {
  char s[60];
  const int buflen = sizeof(s) / sizeof(s[0]);
  time_t t = rtime.sec;
  tm *tm_time = localtime(&t);
  size_t len = strftime(s, buflen, "%b %d %Y %H:%M:%S", tm_time);
  if (len == 0) {
    return string("ERROR MAKING DATE!");
  } else {
    if (len < buflen - 1) {
      snprintf(s + len, buflen - len, ".%d (%0.2f)",
               (int)round(rtime.nsec / 10000000.0), rtime.toSec());
    }
    return string(s);
  }
}
Exemplo n.º 29
0
 void callback(const PointCloud::ConstPtr& cloud)
 {
   if (max_update_rate_ > 0.0)
   {
     NODELET_DEBUG("update set to %f", max_update_rate_);
     if ( last_update_ + ros::Duration(1.0/max_update_rate_) > ros::Time::now())
     {
       NODELET_DEBUG("throttle last update at %f skipping", last_update_.toSec());
       return;
     }
   }
   else
     NODELET_DEBUG("update_rate unset continuing");
   last_update_ = ros::Time::now();
   pub_.publish(cloud);
 }
Exemplo n.º 30
-1
void HeaderManipulation::publishMsg(const topic_tools::ShapeShifter &msg, const ros::Time &msg_in_time)
{
    ROS_DEBUG("HeaderManipulation publishMsg Thread started with timestamp %f", msg_in_time.toSec());
    ros::Time end_time(ros::Time::now());
    ros::Time last_time;
    config_mutex_.lock();
    ros::Time time_to_pub(msg_in_time + msg_delay_);
    config_mutex_.unlock();
    do
    {
        last_time = end_time;
        ROS_DEBUG("waiting to publish msg from %f at %f", msg_in_time.toSec(), time_to_pub.toSec());
        if (end_time > time_to_pub)
        {
            boost::mutex::scoped_lock pub_lock(pub_mutex_);
            ROS_DEBUG("publishing msg which should be held back till: %f", time_to_pub.toSec());
            generic_pub_.publish(msg);
            return;
        }
        boost::mutex::scoped_lock config_lock(config_mutex_);
        publish_retry_rate_.sleep();
        time_to_pub = msg_in_time + msg_delay_;
        end_time = ros::Time::now();
    } while (last_time <= end_time);
    ROS_WARN("Detected jump back in time. Dropping msg. last: %f, end %f",
             last_time.toSec(), end_time.toSec());
    return;
}