所以我制作了機器人，它應該從激光傳感器獲取資料，然后移動到一段距離并停止。
但我發現回呼函式有問題。是否有更好的解釋如何正確使用回呼更新變數？我對 python 有同樣的問題，在那里我發現 time.sleep(0.2) 讓類正確更新。即使這對我來說有點神奇。因為我認為在 python 中這會自動作業，因為分離的執行緒。
在 c 中，我知道基本是使用 spinOnce() 和 spin()。這在正常的非面向物件的情況下應該如何作業。但是在課堂上我又發現課堂沒有正確更新。為什么會出現這種情況？我找不到回呼函式無法正常作業的原因。我可以通過 print full range from reading 來查看是否是這種情況，但它從未發生過。我有 ros::spinOnce() 并且我認為我有正確的成員函式。有人可以幫幫我嗎 ？并幫助我理解？
機器人.h
#include <ros/ros.h>
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/Twist.h"
#include <math.h>
#include <iostream>
class Robot{
geometry_msgs::Twist velocity;
sensor_msgs::LaserScan ls_scan;
ros::Subscriber sub;
ros::Publisher pub;
ros::Rate rate{10};
double speed_linear;
double speed_angular;
public:
Robot(ros::NodeHandle *handle_IN, double rate_IN, double speed_linear_IN,double speed_angular_IN){
rate = ros::Rate{rate_IN};
speed_linear=speed_linear_IN;
speed_angular=speed_angular_IN;
sub = handle_IN->subscribe("/scan",1000,&Robot::scan_callback,this);
pub = handle_IN->advertise<geometry_msgs::Twist>("/cmd_vel",10);
usleep(2000000);
}
void scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg);
void move();
void rotate(bool clock_wise);
void stop();
bool obstacle_in_front(double distance);
bool can_drive(double distance);
std::vector<float> front_read();
std::vector<float> back_read();
};
機器人.cpp
#include "robot.h"
void Robot::scan_callback(const sensor_msgs::LaserScan::ConstPtr& msg){
ls_scan = *msg;
for(float x: ls_scan.ranges)
std::cout << x;
std::cout << std::endl;
}
void Robot::move(){
velocity = geometry_msgs::Twist();
velocity.linear.x = speed_linear;
ros::spinOnce();
while(!obstacle_in_front(0.56)){
ros::spinOnce();
std::cout << "moving\n";
pub.publish(velocity);
rate.sleep();
}
stop();
}
void Robot::rotate(bool clock_wise){
velocity = geometry_msgs::Twist();
velocity.angular.z = speed_angular;
ros::spinOnce();
while(can_drive(2)){
ros::spinOnce();
std::cout << "rotating\n";
pub.publish(velocity);
rate.sleep();
}
stop();
}
void Robot::stop(){
std::cout << "stop\n";
velocity = geometry_msgs::Twist();
pub.publish(velocity);
}
float min(const std::vector<float>& v){
if(v.size() == 0)
return -1;
float min = v[0];
for(float x: v){
if(x < min)
min = x;
}
return min;
}
std::vector<float> Robot::front_read(){
ros::spinOnce();
std::vector<float> front(20);
if(ls_scan.ranges.size()<350)
return front;
for(int i = 0; i < 10; i)
front.push_back(ls_scan.ranges[i]);
for(int i = 350; i < 360; i)
front.push_back(ls_scan.ranges[i]);
return front;
}
std::vector<float> Robot::back_read(){
ros::spinOnce();
std::vector<float> front(20);
if(ls_scan.ranges.size()<350)
return front;
for(int i = 169; i < 190; i)
front.push_back(ls_scan.ranges[i]);
return front;
}
bool Robot::obstacle_in_front(double distance){
float minN = min(front_read());
if(minN > distance)
return false;
else
return true;
std::cout << minN << "\n";
}
bool Robot::can_drive(double distance){
float minN = min(front_read());
if(minN > distance)
return true;
else
return false;
}
機器人控制節點.cpp
#include "robot.h"
int main(int argc, char **argv){
ros::init(argc,argv, "robot_node_node");
ros::NodeHandle n;
Robot robot(&n,10,0.5,0.3);
robot.move();
}
uj5u.com熱心網友回復：
我發現了問題。 基本上帶有回呼。你必須確定，出版商趕上了。所以在你呼叫 spinOnce() 之前，它會檢查它是否存在。您必須呼叫某種等待功能。ros::rate/ros::Duration 并等待。然后當你呼叫 spinOnce() 時。您將獲得新的傳入資料，回呼函式可以讀取這些資料。
在這個意義上：
std::vector<float> Robot::front_read(){
ros::Duration(0.1).sleep(); //wait for new callback
ros::spinOnce(); //read the callback
std::vector<float> front;
if(ls_scan.ranges.size()<350)
return front;
for(int i = 0; i < 10; i)
front.push_back(ls_scan.ranges[i]);
for(int i = 350; i < 360; i)
front.push_back(ls_scan.ranges[i]);
return front;
}
uj5u.com熱心網友回復：
所以最后的答案是這樣的。
- 等待有效資料 -> 我使用簡單的等待回圈來執行此操作，該回圈在建構式中初始化發布者/訂閱者之后運行。
- 然后，您可以在需要時檢查新的有效資料。示例是在 move 成員函式中呼叫 spinOnce()。這是通過 do-while 完成的，因為您想在新資料之后檢查條件。
void Robot::wait_for_valid_data(){
while(ls_scan.ranges.size() < 359)
{
std::cout << "waiting for valid data\n";
ros::spinOnce();
rate.sleep();
}
std::cout << "valid data";
}
void Robot::move(){
velocity = geometry_msgs::Twist();
velocity.linear.x = speed_linear;
do{
if(!ros::ok())
break;
std::cout << "moving\n";
pub.publish(velocity);
ros::spinOnce();
rate.sleep();
}while(!obstacle_in_front(0.56));
stop();
}
轉載請註明出處，本文鏈接：https://www.uj5u.com/qukuanlian/411688.html
標籤：