안녕하세요.
ros melodic 에서 cyglidar 를 사용하고 있는 중입니다.
공식문서(https://www.cygbot.com/_files/ugd/f5911d_4f320c2e4e2e4a449925cf6aa083873c.pdf)에 따르면 배열이 160행 60열 이라고 표시되어 있는 것 같습니다.
(0, 0) , (0, 1), (0, 2), … , (159, 59) 순 출력이 아닌
(0, 0) , (0, 1), (0, 2), … , (59, 159) 가 아닌가 싶습니다
가로 방향이 160개 세로 방향이 60개라고 이해했는데 혹시제가 잘못이해한건지요. 확인한번 부탁드립니다.
아래는 제가 작성한 subscriber code 입니다.
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <sensor_msgs/LaserScan.h>
#include <cmath>
#include <math.h>
#include <iostream>
#include <typeinfo>
using namespace std;
int spinCnt = 0;
입을 명시
void scanCallback(sensor_msgs::PointCloud2ConstPtr const& msg)
{
system("clear");
int a = 0;
double distance = 0;
double distSum = 0;
int count = 0;
int zeroCnt = 0;
int roopCnt = 0;
double pointArr[60][160];
double pcArr[60][160];
for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it) {
cout << fixed;
cout.precision(2);
// TODO: do something with the values of x, y, z
distance = sqrt(it[0] * it[0] + it[1] * it[1] + it[2] * it[2]);
int itrow = roopCnt / 160;
int itcol = roopCnt % 160;
pointArr[itrow][itcol] = distance;
int itpcRow = roopCnt % 60;
int itpcCol = roopCnt / 60;
pcArr[itpcRow][itpcCol] = distance;
// cout << itrow << ' ' << itcol << '\n';
if(itrow >= 75 && itrow < 85 && itcol >= 25 && itcol < 35){
// cout << distance << ' ';
// if((roopCnt + 1) % 10 == 0) cout << '\n';
if(distance != 0){
// cout << "distplus" << '\n';
distSum += distance;
}else{
// cout << "@@@@" << '\n';
zeroCnt++;
}
}
// if(distance <= 0.5 && distance != 0){
// count++;
// }
// if(distance == 0){
// zeroCnt++;
// }
// if(distance != 0){
// distSum += distance;
// }
roopCnt++;
}
// cout << distSum / zeroCnt << ", " << count / (double)(9600 - zeroCnt) * 100 << '\n';
double transArr[60][160];
cout << "========================================================\n";
spinCnt++;
cout << "spin 횟수 : " << spinCnt << '\n';
cout << "왼쪽에서 오른쪽 순으로 들어온다고 가정한 행렬 \n";
for(int i = 0; i < 60; i++){
for(int j = 0; j < 160; j++){
if(i%2==0 && j%2==0) cout << pointArr[i][j] << ' ';
// transArr[j][i] = pointArr[i][j];
}
if(i%2==0) cout << '\n';
}
cout << "위에서 아래 순으로 들어온다고 가정한 행렬 \n";
for(int i = 0; i < 60; i++){
for(int j = 0; j < 160; j++){
if(i%2 == 0 && j%4 == 0)cout << pcArr[i][j] << ' ';
}
if(i%2 == 0)cout << '\n';
}
double dist = distSum / (double)(100 - zeroCnt);
// if(zeroCnt != 100) cout << dist << "\n";
// else cout << -1 << '\n';
// system("clear");
}
int main(int argc, char **argv)
{
// ros::init(argc, argv, "listener"); // 노드 이름 초기화
// ros::NodeHandle nh;
// // 서브스크라이버 선언
// //// 토픽 이름: chatter(publish 노드에서 주는 토픽), 큐 사이즈: 100개
// //// 콜백 함수 이름: chatterCallback(위에서 정의함, 여기서 받은 토픽을 처리함)
// ros::Subscriber sub = nh.subscribe("/scan_3D", 100, chatterCallback);
// ros::spin(); // 큐에 요청된 콜백함수를 처리하며, 프로그램 종료시까지 반복함
// return 0;
ros::init(argc, argv, "pointcloud2_subscriber");
ros::NodeHandle nh;
ros::Subscriber scan_sub = nh.subscribe<sensor_msgs::PointCloud2>("scan_3D", 5, scanCallback);
ros::spin();
return 0;
}
안녕하세요.
ros melodic 에서 cyglidar 를 사용하고 있는 중입니다.
공식문서(https://www.cygbot.com/_files/ugd/f5911d_4f320c2e4e2e4a449925cf6aa083873c.pdf)에 따르면 배열이 160행 60열 이라고 표시되어 있는 것 같습니다.
(0, 0) , (0, 1), (0, 2), … , (159, 59) 순 출력이 아닌
(0, 0) , (0, 1), (0, 2), … , (59, 159) 가 아닌가 싶습니다
가로 방향이 160개 세로 방향이 60개라고 이해했는데 혹시제가 잘못이해한건지요. 확인한번 부탁드립니다.
아래는 제가 작성한 subscriber code 입니다.