C++ Time::toNSec方法代码示例

void writeCSVCamera(shared_ptr<ofstream> file, ros::Time stamp)
{
  std::stringstream ss;
  ss << stamp.toNSec() << "," << stamp.toNSec() << ".png";

  *file << ss.str() << endl;
}

	/**
	 * Send transform to FCU position controller
	 *
	 * Note: send only XYZ, Yaw
	 */
	void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
		// ENU frame
		tf::Vector3 origin = transform.getOrigin();
		tf::Quaternion q = transform.getRotation();

		/* Documentation start from bit 1 instead 0,
		 * Ignore velocity and accel vectors, yaw rate
		 */
		uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);

		if (uas->is_px4()) {
			/**
			 * Current PX4 has bug: it cuts throttle if there no velocity sp
			 * Issue #273.
			 *
			 * @todo Revesit this quirk later. Should be fixed in firmware.
			 */
			ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
		}

		// ENU->NED. Issue #49.
		set_position_target_local_ned(stamp.toNSec() / 1000000,
				MAV_FRAME_LOCAL_NED,
				ignore_all_except_xyz_y,
				origin.y(), origin.x(), -origin.z(),
				0.0, 0.0, 0.0,
				0.0, 0.0, 0.0,
				tf::getYaw(q), 0.0);
	}

int main(int argc, char **argv){

    ros::init(argc, argv, "remaining_memory");
	 ros::NodeHandle n;

    //Publisher part

    ros::Publisher remaining_memory_pub = n.advertise<mavros_msgs::Mavlink>("mavlink/to", 2000);
    ros::Rate loop_rate(10);
    
    FILE *in;
	 char buff[512];
    float remaining_memory = 0.0;

    int count = 0;
    while (ros::ok()) {

       // get the remaining memory thanks to a shell script

	    if(!(in = popen("df -h /home/ | xargs | cut -d ' ' -f 14 | sed 's/%//g'", "r"))){
		     exit(1);
	    }

	    while(fgets(buff, sizeof(buff), in)!=NULL){
		     remaining_memory = (float) atoll(buff);
	    }
	    pclose(in);


        //message generation
        //1. make named value float with type mavlink_message_t
        mavlink::mavlink_message_t msg;
        mavlink::MsgMap map(msg);

	mavlink::common::msg::NAMED_VALUE_FLOAT mem_msg;

	std::array<char, 10> name = {"rem_mem"};

	mem_msg.time_boot_ms = stamp.toNSec()/1000;
	mem_msg.name = name;
	mem_msg.value = remaining_memory;


	mem_msg.serialize(map);

        auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();

        rmsg->header.stamp = ros::Time::now();
        mavros_msgs::mavlink::convert(msg, *rmsg);

		  remaining_memory_pub.publish(rmsg);
		  ros::spinOnce();

		  loop_rate.sleep();
	 	  ++count;
	 }

    return 0;

}

bool ElevationMap::update(const grid_map::Matrix& varianceUpdate, const grid_map::Matrix& horizontalVarianceUpdateX,
                          const grid_map::Matrix& horizontalVarianceUpdateY,
                          const grid_map::Matrix& horizontalVarianceUpdateXY, const ros::Time& time)
{
  boost::recursive_mutex::scoped_lock scopedLock(rawMapMutex_);

  const auto& size = rawMap_.getSize();

  if (!((Index(varianceUpdate.rows(), varianceUpdate.cols()) == size).all()
      && (Index(horizontalVarianceUpdateX.rows(), horizontalVarianceUpdateX.cols()) == size).all()
      && (Index(horizontalVarianceUpdateY.rows(), horizontalVarianceUpdateY.cols()) == size).all()
      && (Index(horizontalVarianceUpdateXY.rows(), horizontalVarianceUpdateXY.cols()) == size).all())) {
    ROS_ERROR("The size of the update matrices does not match.");
    return false;
  }

  rawMap_.get("variance") += varianceUpdate;
  rawMap_.get("horizontal_variance_x") += horizontalVarianceUpdateX;
  rawMap_.get("horizontal_variance_y") += horizontalVarianceUpdateY;
  rawMap_.get("horizontal_variance_xy") += horizontalVarianceUpdateXY;
  clean();
  rawMap_.setTimestamp(time.toNSec());

  return true;
}

void revert_applied_readings_since(const ros::Time& time)
{
    int msec = (int)(time.toNSec()/1000l);

    int k = 0;

    for(std::vector<ras_arduino_msgs::Encoders>::reverse_iterator it = _ringbuffer.rbegin(); it != _ringbuffer.rend(); ++it)
    {
        ras_arduino_msgs::Encoders& enc = *it;
        if (enc.timestamp >= msec)
        {
            ++k;

            double dist_l = (2.0*M_PI*robot::dim::wheel_radius) * (enc.delta_encoder1 / robot::prop::ticks_per_rev);
            double dist_r = (2.0*M_PI*robot::dim::wheel_radius) * (enc.delta_encoder2 / robot::prop::ticks_per_rev);

            _theta += (dist_r - dist_l) / robot::dim::wheel_distance;

            double dist = (dist_r + dist_l) / 2.0;

            _x += dist * cos(_theta);
            _y += dist * sin(_theta);

            //remove reading
            _ringbuffer.erase(--it.base());
        }
    }

    ROS_ERROR("[PoseGenerator::revertReadingsSince] Reverted %d readings.",k);
}

	/**
	 * @brief Send velocity to FCU velocity controller
	 *
	 * @warning Send only VX VY VZ. ENU frame.
	 */
	void send_setpoint_velocity(const ros::Time &stamp, Eigen::Vector3d &vel_enu, double yaw_rate)
	{
		/**
		 * Documentation start from bit 1 instead 0;
		 * Ignore position and accel vectors, yaw.
		 */
		uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);
		auto vel = [&] () {
			if (mav_frame == MAV_FRAME::BODY_NED || mav_frame == MAV_FRAME::BODY_OFFSET_NED) {
				return ftf::transform_frame_baselink_aircraft(vel_enu);
			} else {
				return ftf::transform_frame_enu_ned(vel_enu);
			}
		} ();

		auto yr = ftf::transform_frame_baselink_aircraft(Eigen::Vector3d(0.0, 0.0, yaw_rate));

		set_position_target_local_ned(stamp.toNSec() / 1000000,
					utils::enum_value(mav_frame),
					ignore_all_except_v_xyz_yr,
					Eigen::Vector3d::Zero(),
					vel,
					Eigen::Vector3d::Zero(),
					0.0, yr.z());
	}

	/**
	 * @brief Send vision speed estimate to FCU velocity controller
	 */
	void send_vision_speed(const geometry_msgs::Vector3 &vel_enu, const ros::Time &stamp) {
		Eigen::Vector3d vel_;
		tf::vectorMsgToEigen(vel_enu, vel_);
		auto vel = UAS::transform_frame_enu_ned(vel_);

		vision_speed_estimate(stamp.toNSec() / 1000,
				vel.x(), vel.y(), vel.z());
	}

 void profilingLoggerCurrentTimeDuration(ros::Time currentTime,
                                         ros::Time startTime,
                                         LogThis &logger,
                                         std::string durationName)
 {
   logger.log(currentTime.toNSec(), "currentTime");
   logger.log((currentTime-startTime).toSec(), durationName);
 }

	/**
	 * @brief Send vision speed estimate to FCU velocity controller
	 */
	void send_vision_speed(const geometry_msgs::Vector3 &vel_enu, const ros::Time &stamp)
	{
		Eigen::Vector3d vel_;
		tf::vectorMsgToEigen(vel_enu, vel_);
		//Transform from ENU to NED frame
		auto vel = ftf::transform_frame_enu_ned(vel_);

		vision_speed_estimate(stamp.toNSec() / 1000, vel);
	}

	/**
	 * Send angular velocity setpoint to FCU attitude controller
	 *
	 * ENU frame.
	 */
	void send_attitude_ang_velocity(const ros::Time &stamp, const float vx, const float vy, const float vz) {
		// Q + Thrust, also bits noumbering started from 1 in docs
		const uint8_t ignore_all_except_rpy = (1<<7)|(1<<6);
		float q[4] = { 1.0, 0.0, 0.0, 0.0 };

		set_attitude_target(stamp.toNSec() / 1000000,
				ignore_all_except_rpy,
				q,
				vy, vx, -vz,
				0.0);
	}

	/**
	 * @brief Send angular velocity setpoint to FCU attitude controller
	 *
	 * @note ENU frame.
	 */
	void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel) {
		/* Q + Thrust, also bits noumbering started from 1 in docs
		 */
		const uint8_t ignore_all_except_rpy = (1 << 7) | (1 << 6);
		float q[4] = { 1.0, 0.0, 0.0, 0.0 };

		auto av = UAS::transform_frame_enu_ned(ang_vel);

		set_attitude_target(stamp.toNSec() / 1000000,
				ignore_all_except_rpy,
				q,
				av.x(), av.y(), av.z(),
				0.0);
	}

void ValterJoyTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
  float linVel = joy->axes[1];
  float angVel = joy->axes[2];

  linVel = linVel * 0.12;
  angVel = angVel * 0.587;

  if (fabs(prevLin - linVel) > 0.005 || fabs(prevAng - angVel) > 0.005 || (joy->axes[0] == 0 && joy->axes[1] == 0 && joy->axes[2] == 0 && joy->axes[3] == 0 && joy->axes[4] == 0 && joy->axes[5] == 0))
  {

    //ROS_INFO("TIME: %f", (ros::Time::now().toNSec() * 1e-6) - (prevSentTime.toNSec() * 1e-6));
    //ROS_INFO("linVel = %f, angVel = %f", linVel, angVel);

    if ((ros::Time::now().toNSec() * 1e-6 - prevSentTime.toNSec() * 1e-6) > 50 || (joy->axes[0] == 0 && joy->axes[1] == 0 && joy->axes[2] == 0 && joy->axes[3] == 0 && joy->axes[4] == 0 && joy->axes[5] == 0))
    {

      std::string cmdVelTaskScriptLine = format("T_PCP1_CmdVelTask_%.2f_%.2f", linVel, angVel);

      ROS_INFO("%s", cmdVelTaskScriptLine.c_str());

      int sock = socket(AF_INET , SOCK_STREAM , 0);
      struct sockaddr_in server;
      server.sin_addr.s_addr = inet_addr("192.168.0.248");
      server.sin_family = AF_INET;
      server.sin_port = htons(55555);

      //Connect to remote server
      if (connect(sock , (struct sockaddr *)&server , sizeof(server)) < 0)
      {
        perror("connect failed. Error");
      }
      else
      {
        //Send some data
        if( send(sock , cmdVelTaskScriptLine.c_str() , strlen( cmdVelTaskScriptLine.c_str() ) , 0) < 0)
        {
            perror("Send failed : ");
        }
        close(sock);
      }


      prevSentTime = ros::Time::now();
      prevLin = linVel;
      prevAng = angVel;
    }
  }
}

	/**
	 * Send velocity to FCU velocity controller
	 *
	 * Note: send only VX VY VZ. ENU frame.
	 */
	void send_setpoint_velocity(const ros::Time &stamp, float vx, float vy, float vz, float yaw_rate) {
		/* Documentation start from bit 1 instead 0,
		 * Ignore position and accel vectors, yaw
		 */
		uint16_t ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0);

		// ENU->NED. Issue #49.
		set_position_target_local_ned(stamp.toNSec() / 1000000,
				MAV_FRAME_LOCAL_NED,
				ignore_all_except_v_xyz_yr,
				0.0, 0.0, 0.0,
				vy, vx, -vz,
				0.0, 0.0, 0.0,
				0.0, yaw_rate);
	}

	/**
	 * @brief Send angular velocity setpoint and thrust to FCU attitude controller
	 */
	void send_attitude_ang_velocity(const ros::Time &stamp, const Eigen::Vector3d &ang_vel, const float thrust)
	{
		/**
		 * @note Q, also bits noumbering started from 1 in docs
		 */
		const uint8_t ignore_all_except_rpy = (1 << 7);

		auto av = ftf::transform_frame_baselink_aircraft(ang_vel);

		set_attitude_target(stamp.toNSec() / 1000000,
					ignore_all_except_rpy,
					Eigen::Quaterniond::Identity(),
					av,
					thrust);
	}

	/**
	 * @brief Send attitude setpoint to FCU attitude controller
	 *
	 * @note ENU frame.
	 */
	void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
		/* Thrust + RPY, also bits numbering started from 1 in docs
		 */
		const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
		float q[4];

		UAS::quaternion_to_mavlink(
				UAS::transform_orientation_enu_ned(
					UAS::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))),q);

		set_attitude_target(stamp.toNSec() / 1000000,
				ignore_all_except_q,
				q,
				0.0, 0.0, 0.0,
				0.0);
	}