evarobot_gazebo using the topic /lidar in c++ node
Hello, I have a problem reading values from the topic lidar in via a node:
Firstly I execute "roslaunch evarobot_gazebo evarobot.launch". After that I can read values by "rostopic echo /lidar -n1" command in another terminal, without a problem and it prints out 720 different distance values
But when it comes to my node it can get only 6 different distance values. And my node doesn't get effected by obstacles so it reads always as it's "inf" value.
I had also tried the code lines in "https://answers.ros.org/question/60239/how-to-extract-and-use-laser-data-from-scan-topic-in-ros-using-c/" but it gave compile error.
The problem documentation: "https://drive.google.com/open?id=1MHL9s7wWuKziYhN4nqojooWMplPTxX5z".
ulidar is the package in ~/catkin_ws/src/.
ps: I'm using ros-kinetic-desktop-full.
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
//#include "sensor_msgs/Range.h"
//#include "std_msgs/String.h" //old
#include <math.h>
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/LaserEcho.h"
#define Uabs(x) ((x>0)?(x):(-x))
using namespace std;
struct vektor2d{
double x;
double y;
};
typedef struct vektor2d an2dVektor;
ros::Publisher theVelPub;
ros::Publisher theLaserEo;
void chtrCallBack(const sensor_msgs::LaserScan::ConstPtr& msgLsr);
int main(int argc, char ** argv)
{
ros::init(argc,argv,"apf_lidar");
ros::NodeHandle n;
theVelPub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
/*theLaserEo = n.advertise<sensor_msgs::LaserEcho>("/lidar",10);
sensor_msgs::LaserEcho lEcho;
cout<<endl<<size:<<sizeof(lEcho.echoes)/sizeof(lEcho.echoes[0])<<endl;*/
//theLaserEo.publish(lEcho);
ros::Subscriber scanSub;
scanSub=n.subscribe<sensor_msgs::LaserScan>("/lidar",1,chtrCallBack);
int num=1;
ros::Rate loop_rate(100);
while(num--)
loop_rate.sleep(), ros::spinOnce();
return 0;
}
void chtrCallBack(const sensor_msgs::LaserScan::ConstPtr& msgLsr)
{
cout<<endl<<"What's going on?"<<endl;
int size=sizeof(msgLsr->ranges)/sizeof(msgLsr->ranges[0]);
cout<<"Number of elements of array ranges:"<<size<<endl;
while(size--)
cout<<"ranges["<<size<<"]: "<<msgLsr->ranges[size]<<endl;
cout<<"End of the CallBack"<<endl;
}
gives the output:
What's going on?
Number of elements of array ranges:6
ranges[5]: inf
ranges[4]: inf
ranges[3]: inf
ranges[2]: inf
ranges[1]: inf
ranges[0]: inf
End of the CallBack
Much obliged...
Asked by umut on 2019-02-16 09:29:01 UTC
Answers
The ranges element is a std::vector
int size = msgLsr->ranges.size();
This will give you the correct vector length. It's also quite possible that the first 6 readings are out of range (too close or too far). So hopefully fixing the size will get this working.
Let us know how you get on.
Asked by PeteBlackerThe3rd on 2019-02-17 05:45:24 UTC
Comments