void MoveSlowAndClear::distanceCheck(const ros::TimerEvent& e)
{
if(limited_distance_ * limited_distance_ <= getSqDistance())
{
ROS_INFO("Moved far enough, removing speed limit.");
//have to do this because a system call within a timer cb does not seem to play nice
if(remove_limit_thread_)
{
remove_limit_thread_->join();
delete remove_limit_thread_;
}
remove_limit_thread_ = new boost::thread(boost::bind(&MoveSlowAndClear::removeSpeedLimit, this));
distance_check_timer_.stop();
}
}
double CvHelper::getSqDistance(cv::Point3f p1, cv::Point3f p2)
{
return getSqDistance(p1.x, p1.y, p1.z, p2.x, p2.y, p2.z);
}
