void simulateOneCycle()
{
tf::StampedTransform transform;
try{
listener.waitForTransform("map","EEframe", ros::Time(0), ros::Duration(1.0));
listener.lookupTransform( "map","EEframe", ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
// This is correct, but does not work.
// state_ = transform;
KDL::Frame state_copy;
tf::transformTFToKDL(state_, state_copy);
nh_.getParamCached("/cds_controller_pr2/run_controller", run_controller_flag);
if (run_controller_flag == 1.0){ // Controller running -> Update next desired position
tf::transformKDLToTF(cds_.update(state_copy), state_);
} else {
state_ = transform;
}
broadcastTF();
}
void TopicPublisherROS::updateState(double current_time)
{
if(!enabled_ || !_node) return;
if( !ros::master::check() )
{
QMessageBox::warning(nullptr, tr("Disconnected!"),
"The roscore master cannot be detected.\n"
"The publisher will be disabled.");
_enable_self_action->setChecked(false);
return;
}
//-----------------------------------------------
broadcastTF(current_time);
//-----------------------------------------------
auto data_it = _datamap->user_defined.find( "__consecutive_message_instances__" );
if( data_it != _datamap->user_defined.end() )
{
const PlotDataAny& continuous_msgs = data_it->second;
_previous_play_index = continuous_msgs.getIndexFromX(current_time);
//qDebug() << QString("u: %1").arg( current_index ).arg(current_time, 0, 'f', 4 );
}
for(const auto& data_it: _datamap->user_defined )
{
const std::string& topic_name = data_it.first;
const PlotDataAny& plot_any = data_it.second;
if( !toPublish(topic_name) )
{
continue;// Not selected
}
const RosIntrospection::ShapeShifter* shapeshifter =
RosIntrospectionFactory::get().getShapeShifter( topic_name );
if( shapeshifter->getDataType() == "tf/tfMessage" ||
shapeshifter->getDataType() == "tf2_msgs/TFMessage" )
{
continue;
}
int last_index = plot_any.getIndexFromX( current_time );
if( last_index < 0)
{
continue;
}
const auto& any_value = plot_any.at(last_index).y;
if( any_value.type() == typeid(rosbag::MessageInstance) )
{
const auto& msg_instance = nonstd::any_cast<rosbag::MessageInstance>( any_value );
publishAnyMsg( msg_instance );
}
}
if( _publish_clock )
{
rosgraph_msgs::Clock clock;
try{
clock.clock.fromSec( current_time );
_clock_publisher.publish( clock );
}
catch(...)
{
qDebug() << "error: " << current_time;
}
}
}
void SparseTrackerAM::pointCloudCallback(const PointCloudT::ConstPtr& cloud_in_ptr)
{
boost::mutex::scoped_lock(mutex_);
struct timeval start_callback, end_callback;
struct timeval start_features, end_features;
struct timeval start_model, end_model;
struct timeval start_icp, end_icp;
gettimeofday(&start_callback, NULL);
// **** initialize ***********************************************************
if (!initialized_)
{
initialized_ = getBaseToCameraTf(cloud_in_ptr);
if (!initialized_) return;
}
// **** extract features *****************************************************
gettimeofday(&start_features, NULL);
// **** ab prediction
ros::Time cur_time = cloud_in_ptr->header.stamp;
double dt = (cur_time - prev_time_).toSec();
prev_time_ = cur_time;
tf::Transform predicted_f2b;
double pr_x, pr_y, pr_z, pr_roll, pr_pitch, pr_yaw;
if (use_alpha_beta_)
{
double cx, cy, cz, croll, cpitch, cyaw;
getXYZRPY(f2b_, cx, cy, cz, croll, cpitch, cyaw);
pr_x = cx + v_x_ * dt;
pr_y = cy + v_y_ * dt;
pr_z = cz + v_z_ * dt;
pr_roll = croll + v_roll_ * dt;
pr_pitch = cpitch + v_pitch_ * dt;
pr_yaw = cyaw + v_yaw_ * dt;
btVector3 pr_pos(pr_x, pr_y, pr_z);
btQuaternion pr_q;
pr_q.setRPY(pr_roll, pr_pitch, pr_yaw);
predicted_f2b.setOrigin(pr_pos);
predicted_f2b.setRotation(pr_q);
}
else
{
predicted_f2b = f2b_;
}
PointCloudOrb::Ptr orb_features_ptr = boost::make_shared<PointCloudOrb>();
bool orb_extract_result = extractOrbFeatures(cloud_in_ptr, orb_features_ptr);
pcl::transformPointCloud (*orb_features_ptr , *orb_features_ptr, eigenFromTf(predicted_f2b * b2c_));
orb_features_ptr->header.frame_id = fixed_frame_;
// FIXME - removed canny
PointCloudCanny::Ptr canny_features_ptr = boost::make_shared<PointCloudCanny>();
bool canny_extract_result;// = extractCannyFeatures(cloud_in_ptr, canny_features_ptr);
pcl::transformPointCloud (*canny_features_ptr , *canny_features_ptr, eigenFromTf(predicted_f2b * b2c_));
canny_features_ptr->header.frame_id = fixed_frame_;
gettimeofday(&end_features, NULL);
// **** ICP ******************************
gettimeofday(&start_icp, NULL);
bool orb_icp_result = false;
bool canny_icp_result = false;
if (orb_extract_result)
{
//printf("~ORB~\n");
orb_icp_result = OrbICP(orb_features_ptr);
double eps_roll_ = 0.3;
double eps_pitch_ = 0.3;
double eps_yaw_ = 0.3;
double eps_x_ = 0.3;
double eps_y_ = 0.3;
double eps_z_ = 0.3;
if (orb_icp_result)
{
tf::Transform corr = tfFromEigen(orb_reg_.getFinalTransformation());
// particles add error to motion
double c_x, c_y, c_z, c_roll, c_pitch, c_yaw;
double e_x, e_y, e_z, e_roll, e_pitch, e_yaw;
getXYZRPY(corr, c_x, c_y, c_z, c_roll, c_pitch, c_yaw);
for (int i = 0; i < 20; i++)
{
e_roll = getUrand() * eps_roll_ * c_roll;
e_pitch = getUrand() * eps_pitch_ * c_pitch;
e_yaw = getUrand() * eps_yaw_ * c_yaw;
e_x = getUrand() * eps_x_ * c_x;
e_y = getUrand() * eps_y_ * c_y;
e_z = getUrand() * eps_z_ * c_z;
btVector3 e_pos(c_x + e_x, c_y + e_y, c_z + e_z);
btQuaternion e_q;
e_q.setRPY(c_roll + e_roll, c_pitch + e_pitch, c_yaw + e_yaw);
tf::Transform e_corr;
e_corr.setOrigin(e_pos);
e_corr.setRotation(e_q);
pf2b_[i] = e_corr * pf2b_[i];
}
// end particles
tf::Transform measured_f2b = corr * predicted_f2b;
// **** ab estmation
if (use_alpha_beta_)
{
double m_x, m_y, m_z, m_roll, m_pitch, m_yaw;
getXYZRPY(measured_f2b, m_x, m_y, m_z, m_roll, m_pitch, m_yaw);
// residuals
double r_x = m_x - pr_x;
double r_y = m_y - pr_y;
double r_z = m_z - pr_z;
double r_roll = m_roll - pr_roll;
double r_pitch = m_pitch - pr_pitch;
double r_yaw = m_yaw - pr_yaw;
fixAngleD(r_roll);
fixAngleD(r_pitch);
fixAngleD(r_yaw);
// final position
double f_x = pr_x + alpha_ * r_x;
double f_y = pr_y + alpha_ * r_y;
double f_z = pr_z + alpha_ * r_z;
double f_roll = pr_roll + alpha_ * r_roll;
double f_pitch = pr_pitch + alpha_ * r_pitch;
double f_yaw = pr_yaw + alpha_ * r_yaw;
btVector3 f_pos(f_x, f_y, f_z);
btQuaternion f_q;
f_q.setRPY(f_roll, f_pitch, f_yaw);
f2b_.setOrigin(f_pos);
f2b_.setRotation(f_q);
// final velocity
v_x_ = v_x_ + beta_ * r_x / dt;
v_y_ = v_y_ + beta_ * r_y / dt;
v_z_ = v_z_ + beta_ * r_z / dt;
v_roll_ = v_roll_ + beta_ * r_roll / dt;
v_pitch_ = v_pitch_ + beta_ * r_pitch / dt;
v_yaw_ = v_yaw_ + beta_ * r_yaw / dt;
//printf("VEL: %f, %f, %f\n", v_x_, v_y_, v_z_);
}
else
{
f2b_ = measured_f2b;
}
}
}
if (!orb_icp_result)
printf("ERROR\n");
gettimeofday(&end_icp, NULL);
// **** ADD features to model
gettimeofday(&start_model, NULL);
if (model_->points.size() == 0)
{
*model_ += *orb_features_ptr;
}
else
{
pcl::KdTreeFLANN<PointOrb> tree_model;
tree_model.setInputCloud(model_);
std::vector<int> indices;
std::vector<float> distances;
indices.resize(1);
distances.resize(1);
for (int i = 0; i < orb_features_ptr->points.size(); ++i)
{
PointOrb& p = orb_features_ptr->points[i];
int n_found = tree_model.nearestKSearch(p, 1, indices, distances);
if (n_found == 0)
{
model_->points.push_back(p);
model_->width++;
}
else
{
if ( distances[0] > 0.01)
{
//found far away, insert new one
model_->points.push_back(p);
model_->width++;
}
else
{
// found near, modify old one
//PointOrb& q = model_->points[indices[0]];
//q.x = 0.5*(p.x + q.x);
//q.y = 0.5*(p.y + q.y);
//q.z = 0.5*(p.z + q.z);
}
}
}
}
gettimeofday(&end_model, NULL);
// *** broadcast tf **********************************************************
broadcastTF(cloud_in_ptr);
// *** counter **************************************************************
frame_count_++;
// **** print diagnostics ****************************************************
gettimeofday(&end_callback, NULL);
double features_dur = msDuration(start_features, end_features);
double model_dur = msDuration(start_model, end_model);
double icp_dur = msDuration(start_icp, end_icp);
double callback_dur = msDuration(start_callback, end_callback);
//int model_frames = orb_history_.getSize();
int icp_iterations = orb_reg_.getFinalIterations();
int orb_features_count = orb_features_ptr->points.size();
int canny_features_count =canny_features_ptr->points.size();
int model_size = model_->points.size();
printf("F[%d][%d] %.1f \t M[%d] %.1f \t ICP[%d] %.1f \t TOTAL %.1f\n",
orb_features_count, canny_features_count, features_dur,
model_size, model_dur,
icp_iterations,
icp_dur, callback_dur);
}