void Print::print() {
if (count_ < 5) {
timer_.expires_at(timer_.expires_at() + boost::posix_time::seconds(1));
timer_.async_wait(boost::bind(&Print::print, this));
}
std::cout << "wait count " << count_ << std::endl;
count_++;
}
void start_timer(){
ping_timer->expires_at(ping_timer->expires_at() + ping_interval);
ping_timer->async_wait(boost::bind(
&sAudioReceiver::handle_timer,
this,
boost::asio::placeholders::error
));
}
void deadlineCallback(boost::asio::deadline_timer& t, udp::socket& s)
{
if (t.expires_at() <= boost::asio::deadline_timer::traits_type::now())
{
s.cancel();
t.expires_at(boost::posix_time::pos_infin);
}
t.async_wait(boost::bind(&CompactRIOCommunicationNode::deadlineCallback, this, boost::ref(t), boost::ref(s)));
}
void call_func(const boost::system::error_code&)
{
if (count >= count_max)
return;
++count;
f();
t.expires_at(t.expires_at() + boost::posix_time::millisec(500));
t.async_wait(boost::bind(&a_timer::call_func, this,
boost::asio::placeholders::error));
}
void SenderImpl::scheduleTimer(Sender_weak weak)
{
namespace ptime = boost::posix_time;
m_timer.expires_at(m_timer.expires_at() +
ptime::milliseconds(m_config->period()));
m_timer.async_wait(
boost::bind(&SenderImpl::handleTimeout,
this, weak, _1));
}
void print() {
if (m_count >= 5)
return;
std::cout << __func__ << ", m_count=" << m_count << " - " << strDate() << "\n";
m_count += 1;
m_timer.expires_at( m_timer.expires_at() + boost::posix_time::seconds(1) );
m_timer.async_wait(boost::bind(&MyClass::print, this));
//m_timer.async_wait(boost::bind(&My
}
void generate_report(const bs::error_code &ec)
{
if (!ec) {
string report = "\n";
set<Client*>::iterator it;
for (it = clients.begin(); it != clients.end(); ++it)
report += (*it)->make_report();
for (it = clients.begin(); it != clients.end(); ++it)
(*it)->send_report(report);
//std::cerr << report.c_str();
report_timer.expires_at(report_timer.expires_at() + boost::posix_time::seconds(1));
report_timer.async_wait(generate_report);
}
}
void callback(const boost::system::error_code& ec)
{
callback_finisher _(this);
if (ec == boost::asio::error::operation_aborted)
{
_completed.set();
return;
}
_proc(_ctx);
if (_repeat)
{
_timer.expires_at(_timer.expires_at() + boost::posix_time::milliseconds(_ms));
_timer.async_wait(boost::bind(&linux_timer::callback, this, boost::asio::placeholders::error));
}
else
{
_completed.set();
}
}
void checkDeadLine()
{
if (deadline_.expires_at() <= boost::asio::deadline_timer::traits_type::now())
{
if (yawAdj_ != 0.0 || heigthAdj_ != 0.0) {
drone_.Yaw(drone_.Yaw() + yawAdj_);
drone_.H(drone_.H() + heigthAdj_);
drone_.SendCmd();
}
deadline_.expires_from_now(boost::posix_time::milliseconds(50));
}
deadline_.async_wait(boost::bind(&JoystickDroneHandler::checkDeadLine, this));
}
void check_connections(const bs::error_code &ec)
{
if (!ec) {
set<Client*>::iterator it;
vector<Client*> clients_to_remove;
for (it = clients.begin(); it != clients.end(); ++it)
if ((*it)->is_connected() == true || (*it)->is_broken() == true) {
if ((*it)->is_active() == false || (*it)->is_broken() == true)
clients_to_remove.push_back(*it);
else
(*it)->set_active(false);
}
while (!clients_to_remove.empty()) {
remove_client(clients_to_remove.back());
clients_to_remove.pop_back();
}
connection_timer.expires_at(connection_timer.expires_at() + boost::posix_time::seconds(1));
connection_timer.async_wait(check_connections);
}
}
timer() : t(io_context) { t.expires_at(boost::posix_time::pos_infin); }
timer() : t(io_service) { t.expires_at(boost::posix_time::pos_infin); }
// Helper function to get a timer's expiry time.
static boost::asio::deadline_timer::time_type expiry(
const boost::asio::deadline_timer& timer)
{
return timer.expires_at();
}
int init()
{
// Use global namespace for node
node_ = ros::NodeHandlePtr(new ros::NodeHandle());
// Get tf_prefix from global namespace
node_->param("tf_prefix", tf_prefix_, std::string(""));
// Use private namespace for parameters
pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
pnode_->param("publish_rate", publish_rate_, PUBLISH_RATE);
pnode_->param("fixed_frame_id", fixed_frame_id_, std::string("odom"));
fixed_frame_id_ = tf::resolve(tf_prefix_, fixed_frame_id_);
pnode_->param("publish_odom_tf", publish_odom_tf_, true);
pnode_->param("crio/ip", crio_ip_, std::string("127.0.0.1"));
pnode_->param("crio/command_port", crio_cmd_port_, std::string("39000"));
pnode_->param("crio/state_port", crio_state_port_, std::string("39001"));
pnode_->param("crio/socket_timeout", socket_timeout_, 10);
VehicleKinematics::Parameters kin_params;
tfScalar minimum_radius_outer;
if (!pnode_->getParam("kinematics/frame_id", kin_params.frame_id))
{
ROS_WARN_STREAM(pnode_->getNamespace() << "/kinematics/frame_id parameter is not set");
}
else
{
kin_params.frame_id = tf::resolve(tf_prefix_, kin_params.frame_id);
}
if (!pnode_->getParam("kinematics/wheelbase", kin_params.wheelbase_length))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/wheelbase parameter is not set");
return -1;
}
if (!pnode_->getParam("kinematics/track", kin_params.track_width))
{
ROS_FATAL_STREAM( pnode_->getNamespace() << "/kinematics/track parameter is not set");
return -1;
}
if (!pnode_->getParam("kinematics/rotation_center", kin_params.rotation_center))
{
ROS_WARN_STREAM(
pnode_->getNamespace()
<< "/kinematics/rotation_center parameter is not set. Using default: wheelbase/2 = "
<< kin_params.wheelbase_length / 2);
kin_params.rotation_center = kin_params.wheelbase_length / 2;
}
if (!pnode_->getParam("kinematics/minimum_radius_outer", minimum_radius_outer))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/minimum_radius_outer parameter is not set");
return -1;
}
else
{
kin_params.minimum_radius = minimum_radius_outer - kin_params.track_width / 2;
}
if (!pnode_->getParam("kinematics/steering_ratio", kin_params.steering_ratio))
{
ROS_FATAL_STREAM(pnode_->getNamespace() << "/kinematics/steering_ratio parameter is not set");
return -1;
}
// kin_(2.7, 1.626, 1.35);
kin_ = boost::make_shared<VehicleKinematics>(kin_params);
sub_ = node_->subscribe<kut_ugv_msgs::MotionCommandStamped>("motion_command", 200,
&CompactRIOCommunicationNode::motionCommand, this);
odom_pub_ = node_->advertise<nav_msgs::Odometry>("odom", 200);
state_pub_ = node_->advertise<kut_ugv_vehicle::StateStamped>("state", 200);
tf_br_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster);
timeout_ = boost::posix_time::seconds(socket_timeout_);
send_ep_ = udp::endpoint(boost::asio::ip::address::from_string(crio_ip_), std::atoi(crio_cmd_port_.c_str()));
receive_ep_ = udp::endpoint(udp::v4(), std::atoi(crio_state_port_.c_str()));
socket_.open(udp::v4());
deadline_.expires_at(boost::posix_time::pos_infin);
this->deadlineCallback(deadline_, socket_);
return 0;
}
void set_timer(boost::posix_time::ptime time, boost::function<void(const boost::system::error_code&)> handler) {
t.expires_at(time);
t.async_wait(handler);
}
