/**
* @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();
}
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);
}
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);
}
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
}
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);
}
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;
}
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;
}
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;
}
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;
}
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;
}
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);
}
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;
}
}
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;
}
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;
}
}
}
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();
}
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;
}
}
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;
}
/********** 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;
}
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;
}
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);
}
}
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);
}
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;
}
