void ArenaUnifei::spin() {
ros::Rate loop_rate(10.0);
while (ros::ok()) {
spinOnce();
loop_rate.sleep();
}
}
void visualization::DetectionVisualizer::computeSpinOnce()
{
spinOnce(100);
boost::mutex::scoped_lock bounding_box_lock(obj().update_bounding_box_mutex_);
visualizeBoundingBox();
visualizeSampledGrasps();
bounding_box_lock.unlock();
}
void KeyboardConsoleListener::spin()
{
has_stopped_=false;
while(!has_stopped_)
{
spinOnce();
}
KeyboardConsoleListener::reset();
}
void MocapAlign::spin( )
{
while( 1 )
{
boost::this_thread::interruption_point( );
spinOnce( );
r.sleep( );
}
}
virtual void spin ()
{
ros::Rate rate (30);
while (ros::ok ())
{
spinOnce ();
rate.sleep ();
}
}
char KeyboardConsoleListener::waitForIt(char* c, int size)
{
//std::cout<<"BF waitForIt"<<std::endl;
for(;;)
{
char c_in = spinOnce();
//std::cout << (int)c_in << std::endl;
for(int i=0; i<size; ++i) { if(c_in == c[i]) { return c_in; } }
}
}
void draw_ccw_circle::run()
{
while(ok())
{
motors->set_sides(20, 80, MOTOR_ABSOLUTE);
spinOnce();
loop_rate->sleep();
}
}
void PIKSI::spin( )
{
while( ros::ok( ) )
{
boost::this_thread::interruption_point( );
spinOnce( );
diag.update( );
spin_rate.sleep( );
}
}
int main(int argc, char** argv){
ROS_INFO("Hexacopter driver started...");
// int baudSp = validSpeed(argv[2]);
// int St = 0;
// unsigned char buffer=0;
ros::init(argc,argv,"wrapper");
ros::NodeHandle nh("/mikrokopter/");
//control structure initialization
ros::Subscriber motor_sub = nh.subscribe<mk_msgs::motorCommands>("commands/motor", 1000, motorCallback);
ros::ServiceServer setwp_srv = nh.advertiseService("commands/setwp", setwpCallback);
ros::ServiceServer listwp_srv = nh.advertiseService("commands/listwp", listwpCallback);
ros::ServiceServer setpti_srv = nh.advertiseService("commands/setpoti", setptiCallback);
//Topic initialization v
sensordata_pub = nh.advertise<mk_msgs::sensorData>("sensor/data", 1000);
statedata_pub = nh.advertise<mk_msgs::stateData>("state/data", 1000);
MkDriver driver = new MkDriver();
if (driver.openSerial(argv[1], 230400) < 0)
return -1;
//Navi Error request
//encode('e',2,0, 0);
//ROS_INFO("Request");
//buffer = NAVI_SEND_INTERVAL; //1 byte sending interval ( in 10ms steps )
//encode('o',0,&buffer, 1);
int count = 70;
Rate Loop_rate(1000/DRIVER_LOOP_MS);
while(ros::ok()){
count++;
St = receiveData(); //receiving data from FC
if (St>0){
//ROS_INFO("Decode");
decode(St);
publish();
}
//OSD Data request
if (count >= 70){
//ROS_INFO("Request");
buffer = NAVI_SEND_INTERVAL; //1 byte sending interval ( in 10ms steps )
encode('o',0,&buffer, 1);
count = 0;
}
spinOnce();
}
spin();
return 0;
}
void Coop::spin() {
ROS_INFO("Coop Node is up and running!!!");
ros::Rate loop_rate(10.0);
while (ros::ok()) {
checkLoggedRobots();
checkIddleRobots();
checkTasks();
alocateRobotForAllTasks(robots_, tasks_);
publishTaskState();
spinOnce();
loop_rate.sleep();
}
}
// Make sure that we have an image *and* associated calibration
// data.
void
Tracker::waitForImage()
{
ros::Rate loop_rate(10);
while (!exiting()
&& (!image_.getWidth() || !image_.getHeight())
&& (!info_ || info_->K[0] == 0.))
{
//ROS_INFO_THROTTLE(1, "waiting for a rectified image...");
spinOnce();
loop_rate.sleep();
}
}
void qb_class::spin(){
// 1/step_time is the rate in Hz
ros::Rate loop_rate(1.0 / step_time_);
while(ros::ok()) {
spinOnce();
ros::spinOnce();
loop_rate.sleep();
}
}
virtual void spinOnce ()
{
process ();
spinOnce ();
}
void RCE_Sender::mainloop(void)
{
while (1)
spinOnce();
}
void ROS_server::mainScriptAboutToBeCalled()
{ // When simulation is running, we "spinOnce" here:
spinOnce();
}
void ROS_server::instancePass()
{ // When simulation is not running, we "spinOnce" here:
int simState=simGetSimulationState();
if ((simState&sim_simulation_advancing)==0)
spinOnce();
}
int main(int argc,char**argv)
{
//this is now an array
mtt::TargetList targetList;
t_config config;
t_data full_data;
t_flag flags;
vector<t_clustersPtr> clusters;
vector<t_objectPtr> object;
vector<t_listPtr> list;
visualization_msgs::MarkerArray markersMsg;
// Initialize ROS
init(argc,argv,"mtt");
NodeHandle nh("~");
Subscriber subs_points = nh.subscribe("/points", 1000, PointsHandler);
Publisher target_publisher = nh.advertise<mtt::TargetList>("/targets", 1000);
Publisher markers_publisher= nh.advertise<visualization_msgs::MarkerArray>("/markers", 1000);
Publisher arrow_publisher= nh.advertise<visualization_msgs::MarkerArray>("/arrows", 1000);
init_flags(&flags); //Inits flags values
init_config(&config); //Inits configuration values
cout<<"Start to spin"<<endl;
Rate r(100);
while(ok())
{
spinOnce();
r.sleep();
if(!new_data)
continue;
new_data=false;
//Get data from PointCloud2 to full_data
PointCloud2ToData(pointData,full_data);
//clustering
clustering(full_data,clusters,&config,&flags);
//calc_cluster_props
calc_cluster_props(clusters,full_data);
//clusters2objects
clusters2objects(object,clusters,full_data,config);
calc_object_props(object);
//AssociateObjects
AssociateObjects(list,object,config,flags);
//MotionModelsIteration
MotionModelsIteration(list,config);
//cout<<"Number of targets "<<list.size()<<endl;
clean_objets(object);//clean current objects
//clear target list array
targetList.Targets.clear();
//structure to be fed to array
mtt::Target target;
//build header
target.header.stamp = ros::Time::now();
target.header.frame_id = pointData.header.frame_id;
//trying to draw arrows
visualization_msgs::MarkerArray arrow_arrray;
for(uint i=0; i<list.size(); i++)
{
geometry_msgs::Pose pose;
geometry_msgs::Twist vel;
//header
target.header.seq = list[i]->id;
target.id = list[i]->id;
//velocity
vel.linear.x=list[i]->velocity.velocity_x;
vel.linear.y=list[i]->velocity.velocity_y;
vel.linear.z=0;
target.velocity = vel;
//compute orientation based on velocity.
double theta = atan2(vel.linear.y,vel.linear.x);
geometry_msgs::Quaternion target_orientation =
tf::createQuaternionMsgFromYaw(theta);
//pose
pose.position.x = list[i]->position.estimated_x;
pose.position.y = list[i]->position.estimated_y;
pose.position.z = 0;
pose.orientation = target_orientation;
target.pose = pose;
//feed array with current target
//condition to accept as valit target (procopio change)
double velocity = sqrt(pow(vel.linear.x,2)+
pow(vel.linear.y,2));
if(/*velocity > 0.5 &&*/ velocity < 3.0)
targetList.Targets.push_back(target);
//////////////////////////
//drawing arrow part
if(list[i]->velocity.velocity_module > 0.2 &&
list[i]->velocity.velocity_module < 5.0) {
visualization_msgs::Marker arrow_marker;
arrow_marker.type = visualization_msgs::Marker::ARROW;
arrow_marker.action = visualization_msgs::Marker::ADD;
// Set the frame ID and timestamp.
arrow_marker.header.frame_id = pointData.header.frame_id;
arrow_marker.header.stamp = ros::Time::now();
arrow_marker.color.r = 0;
arrow_marker.color.g = 1;
arrow_marker.color.b = 0;
arrow_marker.color.a = 1;
// arrow_marker.scale.x = ;
arrow_marker.scale.x = list[i]->velocity.velocity_module; //length
arrow_marker.scale.y = 0.1; //width
arrow_marker.scale.z = 0.1; //height
arrow_marker.lifetime = ros::Duration(1.0);
arrow_marker.id = list[i]->id;
// Set the pose of the arrow_marker. This is a full 6DOF pose relative to the frame/time specified in the header
arrow_marker.pose = pose;
arrow_arrray.markers.push_back(arrow_marker);
//////////////////////////
}
}
target_publisher.publish(targetList);
CreateMarkers(markersMsg.markers,targetList,list);
markers_publisher.publish(markersMsg);
arrow_publisher.publish(arrow_arrray);
flags.fi=false;
}
return 0;
}
void Tracker::spin()
{
ros::Rate loopRateTracking(100);
tf::Transform transform;
std_msgs::Header lastHeader;
while (!exiting())
{
// When a camera sequence is played several times,
// the seq id will decrease, in this case we want to
// continue the tracking.
if (header_.seq < lastHeader.seq)
lastTrackedImage_ = 0;
if (lastTrackedImage_ < header_.seq)
{
lastTrackedImage_ = header_.seq;
// If we can estimate the camera displacement using tf,
// we update the cMo to compensate for robot motion.
if (compensateRobotMotion_)
try
{
tf::StampedTransform stampedTransform;
listener_.lookupTransform
(header_.frame_id, // camera frame name
header_.stamp, // current image time
header_.frame_id, // camera frame name
lastHeader.stamp, // last processed image time
worldFrameId_, // frame attached to the environment
stampedTransform
);
vpHomogeneousMatrix newMold;
transformToVpHomogeneousMatrix (newMold, stampedTransform);
cMo_ = newMold * cMo_;
mutex_.lock();
tracker_->setPose(image_, cMo_);
mutex_.unlock();
}
catch(tf::TransformException& e)
{
mutex_.unlock();
}
// If we are lost but an estimation of the object position
// is provided, use it to try to reinitialize the system.
if (state_ == LOST)
{
// If the last received message is recent enough,
// use it otherwise do nothing.
if (ros::Time::now () - objectPositionHint_.header.stamp
< ros::Duration (1.))
transformToVpHomogeneousMatrix
(cMo_, objectPositionHint_.transform);
mutex_.lock();
tracker_->setPose(image_, cMo_);
mutex_.unlock();
}
// We try to track the image even if we are lost,
// in the case the tracker recovers...
if (state_ == TRACKING || state_ == LOST)
try
{
mutex_.lock();
// tracker_->setPose(image_, cMo_); // Removed as it is not necessary when the pose is not modified from outside.
tracker_->track(image_);
tracker_->getPose(cMo_);
mutex_.unlock();
}
catch(...)
{
mutex_.unlock();
ROS_WARN_THROTTLE(10, "tracking lost");
state_ = LOST;
}
// Publish the tracking result.
if (state_ == TRACKING)
{
geometry_msgs::Transform transformMsg;
vpHomogeneousMatrixToTransform(transformMsg, cMo_);
// Publish position.
if (transformationPublisher_.getNumSubscribers () > 0)
{
geometry_msgs::TransformStampedPtr objectPosition
(new geometry_msgs::TransformStamped);
objectPosition->header = header_;
objectPosition->child_frame_id = childFrameId_;
objectPosition->transform = transformMsg;
transformationPublisher_.publish(objectPosition);
}
// Publish result.
if (resultPublisher_.getNumSubscribers () > 0)
{
geometry_msgs::PoseWithCovarianceStampedPtr result
(new geometry_msgs::PoseWithCovarianceStamped);
result->header = header_;
result->pose.pose.position.x =
transformMsg.translation.x;
result->pose.pose.position.y =
transformMsg.translation.y;
result->pose.pose.position.z =
transformMsg.translation.z;
result->pose.pose.orientation.x =
transformMsg.rotation.x;
result->pose.pose.orientation.y =
transformMsg.rotation.y;
result->pose.pose.orientation.z =
transformMsg.rotation.z;
result->pose.pose.orientation.w =
transformMsg.rotation.w;
const vpMatrix& covariance =
tracker_->getCovarianceMatrix();
for (unsigned i = 0; i < covariance.getRows(); ++i)
for (unsigned j = 0; j < covariance.getCols(); ++j)
{
unsigned idx = i * covariance.getCols() + j;
if (idx >= 36)
continue;
result->pose.covariance[idx] = covariance[i][j];
}
resultPublisher_.publish(result);
}
// Publish moving edge sites.
if (movingEdgeSitesPublisher_.getNumSubscribers () > 0)
{
visp_tracker::MovingEdgeSitesPtr sites
(new visp_tracker::MovingEdgeSites);
updateMovingEdgeSites(sites);
sites->header = header_;
movingEdgeSitesPublisher_.publish(sites);
}
// Publish KLT points.
if (kltPointsPublisher_.getNumSubscribers () > 0)
{
visp_tracker::KltPointsPtr klt
(new visp_tracker::KltPoints);
updateKltPoints(klt);
klt->header = header_;
kltPointsPublisher_.publish(klt);
}
// Publish to tf.
transform.setOrigin
(tf::Vector3(transformMsg.translation.x,
transformMsg.translation.y,
transformMsg.translation.z));
transform.setRotation
(tf::Quaternion
(transformMsg.rotation.x,
transformMsg.rotation.y,
transformMsg.rotation.z,
transformMsg.rotation.w));
transformBroadcaster_.sendTransform
(tf::StampedTransform
(transform,
header_.stamp,
header_.frame_id,
childFrameId_));
}
}
lastHeader = header_;
spinOnce();
loopRateTracking.sleep();
}
}
int main(int argc,char**argv)
{
mtt::TargetListPC targetList;
t_config config;
t_data full_data;
t_flag flags;
vector<t_clustersPtr> clusters;
vector<t_objectPtr> object;
vector<t_listPtr> list;
visualization_msgs::MarkerArray markersMsg;
// Initialize ROS
init(argc,argv,"mtt");
NodeHandle nh("~");
Subscriber subs_points = nh.subscribe("/points", 1000, PointsHandler);
Publisher target_publisher = nh.advertise<mtt::TargetList>("/targets", 1000);
Publisher markers_publisher= nh.advertise<visualization_msgs::MarkerArray>("/markers", 1000);
Publisher marker_publisher= nh.advertise<visualization_msgs::Marker>("/marker", 1000);
init_flags(&flags); //Inits flags values
init_config(&config); //Inits configuration values
cout<<"Start to spin"<<endl;
Rate r(100);
while(ok())
{
spinOnce();
r.sleep();
if(!new_data)
continue;
new_data=false;
//Get data from PointCloud2 to full_data
PointCloud2ToData(pointData,full_data);
//clustering
clustering(full_data,clusters,&config,&flags);
//calc_cluster_props
calc_cluster_props(clusters,full_data);
//clusters2objects
clusters2objects(object,clusters,full_data,config);
calc_object_props(object);
//AssociateObjects
AssociateObjects(list,object,config,flags);
//MotionModelsIteration
MotionModelsIteration(list,config);
// cout<<"Number of targets "<<list.size()<<endl;
clean_objets(object);//clean current objects
targetList.id.clear();
targetList.obstacle_lines.clear();//clear all lines
pcl::PointCloud<pcl::PointXYZ> target_positions;
pcl::PointCloud<pcl::PointXYZ> velocity;
target_positions.header.frame_id = pointData.header.frame_id;
velocity.header.frame_id = pointData.header.frame_id;
targetList.header.stamp = ros::Time::now();
targetList.header.frame_id = pointData.header.frame_id;
for(uint i=0;i<list.size();i++)
{
targetList.id.push_back(list[i]->id);
pcl::PointXYZ position;
position.x = list[i]->position.estimated_x;
position.y = list[i]->position.estimated_y;
position.z = 0;
target_positions.points.push_back(position);
pcl::PointXYZ vel;
vel.x=list[i]->velocity.velocity_x;
vel.y=list[i]->velocity.velocity_y;
vel.z=0;
velocity.points.push_back(vel);
pcl::PointCloud<pcl::PointXYZ> shape;
pcl::PointXYZ line_point;
uint j;
for(j=0;j<list[i]->shape.lines.size();j++)
{
line_point.x=list[i]->shape.lines[j]->xi;
line_point.y=list[i]->shape.lines[j]->yi;
shape.points.push_back(line_point);
}
line_point.x=list[i]->shape.lines[j-1]->xf;
line_point.y=list[i]->shape.lines[j-1]->yf;
sensor_msgs::PointCloud2 shape_cloud;
pcl::toROSMsg(shape,shape_cloud);
targetList.obstacle_lines.push_back(shape_cloud);
}
pcl::toROSMsg(target_positions, targetList.position);
pcl::toROSMsg(velocity, targetList.velocity);
target_publisher.publish(targetList);
CreateMarkers(markersMsg.markers,targetList,list);
//markersMsg.header.frame_id=targetList.header.frame_id;
markers_publisher.publish(markersMsg);
flags.fi=false;
}
return 0;
}
void KeyboardConsoleListener::waitForIt(char c)
{
for(;;) { if (spinOnce() == c) return; };
}
