둥's 이것저것

자바를 통해 그래프를 그려주기 위해서는 그래프를 나타낼 수 있는 라이브러리를 불러오는 과정이 필요하다.

라이브러리는 https://knowm.org/open-source/xchart/ 링크를 통해 다운받을 수 있고 해당 라이브러리를 다운받으면

압축파일 형식의 파일을 다운받게 되는데 이에 대한 압축을 풀어주자.

압축을 풀면 위와 같이 .jar 형식의 파일이 존재하는데 이를 프로젝트에 불러올 수 있도록 셋팅해주자.

그리고 프로젝트 경로에 'buildpath' 폴더를 하나 생성해준 후 해당 경로에 .jar파일을 넣어주자.

그리고 java 프로젝트의 설정을 진행한다.

우선 1. 에 표시된 것과 같이 프로젝트명을 우클릭하고 Properties 항목을 클릭하면 창이 나타난다.

그럼 2~5의 과정을 진행해주고 6의 과정에서 앞에서 buildpath에 지정한 .jar형태의 라이브러리를 불러준다.

위와 같이 xchart-3.8.1.jar 라이브러리가 링크된 것을 확인할 수 있다.

그럼 이제 코드를 입력해보자.

import java.util.function.IntPredicate;
import org.knowm.xchart.QuickChart;
import org.knowm.xchart.SwingWrapper;
import org.knowm.xchart.XYChart;

public class graph {
	
	public static void main(String[] args) {
		double[] xData = new double[] {0.0, 1.0, 2.0};
		double[] yData = new double[] {2.0, 1.0, 0.0}; 
		
		XYChart chart = QuickChart.getChart("Sample Chart", "X", "Y", "y(x)", xData, yData);
		new SwingWrapper(chart).displayChart();
	}



}

결과는 아래와 같다.

그럼 이제 하나의 창에 여러개의 그래프를 그려보자.

import java.util.ArrayList;
import java.util.List;

import org.knowm.xchart.SwingWrapper;
import org.knowm.xchart.XYChart;
import org.knowm.xchart.XYChartBuilder;
import org.knowm.xchart.XYSeries;
import org.knowm.xchart.style.markers.SeriesMarkers;

public class multiple_graph {
	public static void main(String[] args) {
		int numCharts = 4;
		List charts = new ArrayList();
		for(int i=0; i<numCharts; i++) {
			XYChart chart = new XYChartBuilder().xAxisTitle("X").yAxisTitle("Y").width(600).height(400).build();
			chart.getStyler().setYAxisMin(-10.0);
			chart.getStyler().setYAxisMax(10.0);
			XYSeries series = chart.addSeries(""+i,  null, getRandomWalk(200));
			series.setMarker(SeriesMarkers.NONE);
			charts.add(chart);
		}
		new SwingWrapper(charts).displayChartMatrix();
	}
	
	private static double[] getRandomWalk(int numPoints) {
		double[] y = new double[numPoints];
		y[0] = 0;
		for(int i=1; i<y.length; i++) {
			y[i] = y[i-1]+Math.random()-.5;
		}
		return y;
	}

}

자바에서 사용할 수 있는 GUI 객체는 AWT(Abstract Windows Toolkit)과 스윙(Swing)으로 구분됩니다.

AWT는 운영 체제가 제공하는 자원을 이용하여 컴포넌트를 생성하게 되는데 예를들어 윈도우 환경에서 개발을 진행하고 있으면 GUI는 윈도우 기반의 자원을 사용하여 윈도우에 사용되는 아이콘의 모형이나 프레임을 사용할 수 있게 됩니다.

따라서, 여러 플랫폼에서 쉽게 컴포넌트를 제공할 수 있는 장점이 있지만 컴포넌트가 플랫폼에 종속적이므로 실행되는 플랫폼에 따라 컴포넌트의 모습이 달라질 수 있습니다.

스윙은 컴포넌트가 자바로 작성되어 있기 때문에 어떤 플랫폼에서도 일관된 화면을 보여줄 수 있게 됩니다.

그럼 스윙 기반으로 내용을 시작해보도록 하겠습니다.

import javax.swing.*;
public class FrameTest {
	public static void main(String[] args) {
		JFrame f = new JFrame("Frame Test");
		f.setSize(300, 200);
		f.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		f.setVisible(true);
	}

}

우선 클래스를 생성한 후 위의 소스코드를 입력하면 다음과 같이 프레임을 출력하는 결과를 확인할 수 있습니다.

그럼 이제 코드 내용을 자세히 살펴보겠습니다.

import javax.swing.*;

스윙 클래스들은 javax.swing 패키지 안에 들어있습니다. 여기서 javax는 자바 확장 패키지(Java Extension Package)를 의미하고 따라서 스윙을 사용하려면 반드시 javax.swing 패키지를 포함해야 합니다.

JFrame f = new JFrame("Frame Test");

메인문 안에서 new연산자를 이용해 JFrame 객체를 생성합니다. JFrame 클래스 생성자의 매개변수는 프레임의 제목입니다(Frame Test). 결과적으로 참조변수 f가 생성된 객체를 가리키게 됩니다.

f.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

사용자가 프레임의 오른쪽 상단에 있는 close 버튼을 눌렀을 경우에 어떠한 동작을 취할지 설정합니다. 기본 설정은 close 버튼을 누를 경우 현재 프레임만 닫히고 프로그램을 종료하지는 않습니다. 우리는 close버튼을 누를 경우 전체 프로그램을 종료하도록 설정하였습니다.

f.setVisible(true);

마지막 문장은 setvisible(true)을 호출합니다. 이것은 프레임을 화면에 나타내게 만듭니다. 만약 이 문장이 없다면 프레임은 생성되지만 사용사는 볼 수 없습니다. 이렇게 하는 이유는 화면에 프레임이 나타나지 않은 상태에서 다른 컴포넌트를 추가하기 위함입니다.

JAVA에 대한 예제들을 보며 공부를 하다가 다음과 같은 코드를 보게 되었습니다.

long형 변수 2개를 선언해주고 각각의 값을 지정해준 후 이를 출력해주는 간단한 코드입니다.

그런데 line8의 distance 값을 할당해주는 부분에서 365L을 곱해주는 부분이 있습니다.

수를 곱해주는 것이 아닌 수 뒤에 'L'은 무엇을 나타낼까요?

기본적으로 자바는 모든 정수를 int형으로 나타냅니다.

예를들어

int a = 1000000000000;

위의 코드는 컴파일이 되지 않습니다. int로 표현할 수 있는 범위를 벗어난 것이지요.

그럼 자료형의 범위를 넓혀봅시다.

long a = 1000000000000;

long 형 변수의 범위에는 포함되나 위의 코드 역시 에러를 발생시키게 됩니다.

자바는 수를 변수에 저장하기 전에 메모리에 저장하게 되는데 이때, 메모리에 저장하는 형태가 int형 입니다.

그리고 이러한 경우에 long 변수에 int범위 밖의 변수를 할당해주기 위해 값 뒤에 'L'을 붙여주게 됩니다.

long a = 1000000000000L;

그러면 자바에서 이 수를 int형이 아닌 long 형으로 나타내라고 지시를 하게 됩니다.

그리고 이 때, L을 접미사라고 표현합니다.

그럼 자바에서 사용되는 접미사는 'L'밖에 없을까요?

앞의 과정에서 정수가 처리되는 과정을 살펴봤으니 이제 실수가 처리되는 과정을 봅시다.

자바에서는 기본적으로 실수를 double형으로 나타냅니다.

따라서

float a = 1.2;

위의 코드를 살펴보면 자바에서는 먼저 1.2를 double형으로 나타내는데 이를 float변수에 저장할 경우 제대로 값을 저장하지 못한다는 문제가 발생합니다.(double형: 8바이트, float형: 4바이트)

그렇기에 float형 변수에 실수를 저장할 때에는 접미사 F를 붙여주게 됩니다.

즉, 위의 코드는

float a = 1.2F;

위와 같이 표현될 수 있고 JAVA에서는 기본적으로 실수를 double형으로 저장하기 때문에 기본적으로 실수 변수를 double로 선언하는 것이 좋습니다.

우선 자바를 다루기 위해 '자바 개발 도구(JDK: Java Development Kit)를 다운받아 설치합니다.

그리고 자바 프로그램 개발 환경을 갖출 수 있도록 통합 개발환경(IDE)인 Eclipse를 설치합니다.

설치방법은 이미 많은 분들이 업로드해놓은 내용이 있으므로 생략하도록 하겠습니다.

이클립스 설치를 진행하면 위의 이미지에 나타낸 것과 같이 보라색 동그라미 형태의 아이콘이 생성되고 이를 실행시키면 작업공간의 경로를 지정할 수 있는 창이 나타납니다. 이를 통해 작업공간을 지정해줍니다.

이클립스 IDE가 실행되면 'File'탭의 'New' 항목으로 이동한 후 'Java Project'를 통해 새로운 프로젝트를 생성합니다.

위와 같이 프로젝트 설정을 위한 창이 나타나면 동일하게 셋팅을 진행한 후 아래의 'Finish'를 눌러 마무리합니다.

그럼 위와 같이 프로젝트가 생성된 것을 확인할 수 있습니다.

이제 본격적으로 코드 작업을 진행해봅시다.

'File' -> 'New' -> 'Class' 순서로 입력하면 아래와 같은 창이 나타납니다.

'Name' 에는 생성할 클래스의 이름을 지정하여주고 하단의 "public static void main(Stringp[] args)" 를 체크하여

클래스를 생성할때 자동으로 메인문이 생성될 수 있도록 지정해줍시다.

그럼 이제 메인문 안에 코드를 입력하여주고 좌측 상단의 재생버튼같이 생긴 아이콘을 눌러 실행합니다.

기본적으로 터틀봇에서 위치인식을 진행하기 위한 노드는

src/navigation/amcl/src/amcl_node.cpp 에 대한 내용들로 작업된다.

그 후 해당 코드의 생성자가 실행되고 reconfigureCB( )함수가 실행되며 생성자 함수가 종료된다.

그 후에 진행되는 함수들을 나열하면

laserReceived( )  ->  mapReceived( )  ->  handleMapMessage( )   ->  convertMap( )  ->  updatePoseFromServer( )

->  applyInitialPose( )  ->  laserReceived( )  ->  getOdomPose( )  ->  laserReceived( )  -> getOdomPose( )  -> ...

위와 같은 순서로 진행되며 laserReceived와 getOdomPose가 반복되며 실행되게 된다.

중간중간 savePoseToServer( )  함수가 실행되며 초기 위치를 지정해주게 되면 initialPoseReceived  -> handleInitialPoseMessage  ->  getYaw  -> applyInitialPose  ->  laserReceived  ->  getOdomPose  ->  laserReceived -> ...

위의 과정을 진행하며 결국 다시 laserReceived 와 getOdomPose 를 반복하며 주기적으로 savePoseToServer를 실행하게 된다.

그럼 파티클이 수렴하는 과정에는 어떤 함수가 실행될까?

이 과정에서는 amcl_laser.cpp 소스의 UpdateSensor( )  함수와 LikelihoodFieldModel 함수가 실행되게 됨을 확인하였다.

이제 조금 더 자세히 과정을 살펴보자.

amcl_node.cpp 파일에 아래와 같은 부분이 존재한다.

아래의 코드에서 ldata.range_count는 360의 값을 갖고있는 변수로 이는 Lidar센서가 360도 회전하며

1도 회전할때마다 거리를 취득하게 되는 것으로 예상할 수 있다.

즉 아래의 코드는 라이다센서가 1회 회전할때 취득되는 거리들을 ldata 변수에 저장하는 것으로 볼 수 있다.

ldata.range[n][0]은 거리값, ldata.range[n][1]은 해당 거리값이 얻어진 라이다의 회전 각도로 볼 수 있을 것이다.

그리고 최종적으로는 아래의 코드에 나타낸 UpdateSensor( ) 함수와 pf_update_resample( ) 함수를 통해

파티클의 정보를 업데이트하고 resampling하는 과정을 진행하게 된다.

pf_updata_reample( )을 주석처리 하게되면 파티클들이 로봇의 odom에 따라 이동은 하게 되지만

취득되는 Lidar 데이터와 지도를 매칭시키는 과정이 없으므로 운동량에 따라 파티클들이 이동만 하게되고

위치인식을 진행하지 않게 된다.

반면 UpdateSensor( ) 함수를 주석처리 하게 되면 아래 그림과 같이 파티클들이 어느정도 수렴하게 되기는 하지만 정확한 위치를 나타내지 못하고 심한 오차를 갖게 되는 것을 확인할 수 있다.

하지만 resampling단계에서 파티클들의 오차분포를 포함하여 re-distribution하게 되고 이를 통해 resampling을 진행하므로 어느정도 로봇의 운동량을 추정할 수 있게 됨을 확인할 수 있다.(아래 그림에서 파티클들의 방향이 로봇과 어느정도 일치한다는 것을 통해 이를 확인할 수 있다.)

UpdateSensor( ) 함수에서는 어떠한 과정이 진행되기에 이러한 문제가 발생하는 것일까?

UpdateSensor( ) 함수 먼저 살펴보면 이는 아래와 같이 amcl/sensors/amcl_laser.cpp에 위치하고 있고

amcl_laser.cpp에서 실행되는 pf_update_sensor는 pf.c에 해당 코드가 존재하게 된다.

UpdateSensor( )함수의 파라미터로는 파티클필터의 정보와 Lidar의 취득 데이터가 들어가게 되는데

최종적으로 해당 함수는 취득된 data를 토대로 파티클필터의 정보를 업데이트 하기 위함으로 보인다.

위의 코드는 pf.c에 존재하는 pf_update_sensor( ) 함수의 내용이다.

line281에 total 변수를 통해 sample의 weight가 결정되는 것으로 보이는데 total변수는 (*sensor_fn)의 값으로 이는(sensor_data, set)을 통해 값이 지정되는 것으로 보인다. (sensor_fn)은 함수의 파라미터로 pf_sensor_model_fn_t 자료형을 띄고 있는데 이는 아래와 같이 pf.h에서 정의된 것을 확인할 수 있다.

(pf_sensor_model_fn_t 형은 double형의 재정의로 볼 수 있다.)

그럼 double값을 갖는 pf_sensor_model_fn_t 는 sensor_data와 set을 통해 어떻게 값이 결정되는가?

이 값은 amcl_laser.cpp에서 처음 update_sensor 함수가 실행될 때 정해지게 되는데 이 부분을 다시 확인해보면 LikelihoodFieldModel의 값이 인자로 전달되는 것을 확인할 수 있다.

그리고 LikelihoodFieldModel의 값은 동일한 source file에서 정의된 것을 확인할 수 있다.

즉, 위의 과정을 통해 double값을 return하게 되고 이 값이 가중치를 나타내는 값으로 sensor_update 함수에 전달되는 것이다.

그럼 이 LikelihoodFieldModel( ) 함수를 이해하면 파티클의 가중치값이 어떻게 결정되는지 알 수 있을 것이다.

우선 이 과정에서는 반복문을 통해 각 샘플(하나의 파티클)에 대해 파티클의 위치를 받아오고

라이다에서 취득되는 360개의 거리 데이터를 통해 각각의 라이다 끝점의 위치를 파악한다.

라이다 끝점의 위치는 레이저가 반사된 지점으로 이는 로봇의 기준으로 위치를  나타내게 된다.

따라서 로봇의 위치를 기준으로 표시된 라이다의 끝 점을 지도를 중심으로 변환해주는 과정을 진행하고 이 부분은 아래 그림에 나타낸 부분이다.

지도를 기준으로 표현되는 라이다의 끝점 mi, mj 정보를 바탕으로 z값이 계산되고 이를 통해 최종적인 weight값이 계산되는데 여기서 z값이 의미하는 바는 아직 이해가 부족하다.

UpdateSensor()에서 particle filter를 재정의하고 sample들에 대한 가중치를 재정의 한 후

resample()에서 파티클들을 가중치에 따라 재정의하는 것인가?

Navigation을 위해 Intel의 Realsense D435와 T265를 사용한다.

D435는 3차원 데이터를 취득하기 위함이고 T265는 로봇의 현재 위치와 방향을 상대적으로 얻기 위함이다.

Octomap생성 시 Odometry를 활성화시키면 위의 그림처럼 자주색 공이 나타나고 카메라의 방향을 따라 붉은 화살표가 생기는 것을 확인할 수 있다.

즉 Odometry로 넘어가는 메시지의 정보만 파악하면 현재 카메라의 위치와 방향을 알 수 있게 된다.

메시지 이름은 '/t265/odom/sample' 이며 메시지 타입은 'nav_msgs/Odometry'에 해당한다.

메시지로 전달되는 데이터는 아래와 같다.

pose정보를 보면 position와 orientation이 나눠져 있으며 위치와 방향에 대한 파악이 가능한 것을 확인할 수 있다.

나중에는 Tracking Camera를 사용하지 않고 Visual SLAM에 사용되는 Visual Inertial Odometry를 적용하여

오직 RGB-D Camera로만 위치인식을 진행해봐야겠다.

그럼 본격적으로 해당 메시지를 통해 작업을 수행할 수 있도록 x, y, yaw 정보를 받아오는 노드를 작성해보겠다.

우선 nav_msgs/Odometry 메시지가 어떻게 구성되있는지 살펴보자.

opt/ros/kinetic/include에 해당 정보가 존재하니 이를 include하여 작업을 진행하면 될 것으로 보인다.

해당 메시지의 데이터는 pose와 Twist로 구성되는데 pose는 x, y, z로 구성된 position 정보와 x, y, z, w로 구성된 orientation 정보로 이루어져 있다.

위치인식에 필요한 Particle Filter에서는 로봇의 x, y, yaw정보가 요구되므로 이 정보들을 출력해줄 수 있는 노드를 작성해보겠다.

우선 소스코드는 아래와 같이 작성하였다.

#include <ros/ros.h>
#include "nav_msgs/Odometry.h"

void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    std::cout<<"position x: "<<msg->pose.pose.position.x<<"  y: "<<msg->pose.pose.position.y<<"  z: "<<msg->pose.pose.position.z<<std::endl;
    std::cout<<"orientation x: "<<msg->pose.pose.orientation.x<<"  y: "<<msg->pose.pose.orientation.y<<"  z: "<<msg->pose.pose.orientation.z<<"  w: "<<msg->pose.pose.orientation.w<<std::endl;
    std::cout<<"twist linear x: "<<msg->twist.twist.linear.x <<"  y: "<<msg->twist.twist.linear.y<<"  z: "<<msg->twist.twist.linear.z<<std::endl;
    std::cout<<"twist angular x: "<<msg->twist.twist.angular.x<<"  y: "<<msg->twist.twist.angular.y<<"  z: "<<msg->twist.twist.angular.z<<std::endl;
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "pose_listener");
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe("/t265/odom/sample", 1000, poseCallback);
  ros::spin();
}

/t265/odom/sample  메시지를 받아 와 poseCallback 함수를 실행하는 노드로

position, orientation, twist_linear, twist_angular 값들을 모두 확인할 수 있도록 작성하였다.

하지만 위의 코드와 같이 작성하면 /t265/odom/sample 메시지가 너무 빠르게 전달되므로

값을 확인하기에 무리가 있어 이를 아래와 같이 일부 수정하여 딜레이를 갖도록 구성하였다.

위의 코드를 통해 확인해보니 출력되는 position의 x, y, z는 아래와 같은 구성을 갖고있다.

그리고 Orientation은 Quaternion 좌표계로 3차원 공간상의 이동을 나타내는 것으로 보이나 Quaternion에 대한 이해가 부족하기에 자세히 설명하긴 어렵다.

결론적으로 보면 Quaternion 좌표계는 3차원의 벡터로 표시될 수 있는 방향성을 나타내어주고

ROS의 TF 라이브러리에서는 이를 오일러각(Roll, Pitch, Yaw)으로 변환할 수 있는 함수를 제공해주므로

우리는 위치인식에 필요한 x, y, yaw정보를 취득할 수 있다.

최종적인 코드는 아래와 같다.

#include <ros/ros.h>
#include "nav_msgs/Odometry.h"
#include <tf/tf.h>
#include <geometry_msgs/Pose2D.h>

ros::Publisher pub_pose_;

void poseCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
  geometry_msgs::Pose2D pose2d;
  pose2d.x = msg->pose.pose.position.x;
  pose2d.y = msg->pose.pose.position.y;
  tf::Quaternion q(
    msg->pose.pose.orientation.x,
    msg->pose.pose.orientation.y,
    msg->pose.pose.orientation.z,
    msg->pose.pose.orientation.w
  );
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  pose2d.theta = yaw;
  std::cout<<"yaw: "<<yaw<<"  pitch: "<<pitch<<"  roll: "<<roll<<std::endl;
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "pose_listener");
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe("/t265/odom/sample", 1000, poseCallback);

  ros::Rate loop_rate(0.5);
  while(nh.ok()){
    ros::spinOnce();
    loop_rate.sleep();
  }
}

RGB-D 카메라를 통해 Octomap생성까지 완료되었고 이를 통해 모바일로봇이 주행가능한 Gridmap생성까지 완료되었다.

이제 모바일로봇이 해당 GridMap을 통해 위치인식을 진행하고 경로를 계획하여 주행하는 단계를 진행하면 된다.

본 게시물에서는 우선 GridMap을 통해 위치인식을 진행하기 위한 과정을 다룬다.

-----------------------------------

우선 최종적으로 Gridmap은 경로를 계획하고 위치인식을 진행하기 위해 사용된다.

그렇기에 위치인식을 진행하는 AMCL 패키지에 이를 위한 노드를 생성하려 한다.

위와 같이 src 경로에 cpp파일을 생성하고 아래와 같이 CMakeLists.txt를 수정한다.

amcl_rgbd_node.cpp 파일은 RGB-D 카메라를 통해 정보를 받아와 지도에서 위치인식을 수행할 수 있도록 하는 노드를 생성하게 된다.

이를 위해 2.5차원의 지도를 불러와 Image Processing을 진행할 수 있어야 하고 이를 위해 별도의 노드를 만들어 작업을 진행해 보았다. 이 노드를 안정화한 후 위에서 생성한 amcl_rgbd_node.cpp 파일에 내용을 추가하는 방식으로 작업을 진행할 계획이다.

GridMap의 Message를 받아와 작업을 진행하기 위해 work space에 catkin_ws_map-processing을 만들어 OpenCV 라이브러리를 ROS에서 작업할 수 있는 환경을 갖추었다. 

Workspace 경로에서

$ catkin_create_pkg 명령어를 통해 패키지를 생성하고

CMakelists는 OpenCV를 처리할 수 있도록 다음 내용들을 추가해줬다.

그럼 이제 본격적으로 메시지를 받아와 이를 처리하는 노드를 만들어보자.

위와 같이 Octomap을 실행한 후 GridMap을 통해 2.5차원 지도를 생성하게 되고 2.5차원 지도에 대한 메시지는

아래 결과에서 볼 수 있듯히 /grid_map_visualization/elevation_grid  의 토픽으로 전달되고 있음을 확인할 수 있다.

그럼 Subscriber는 해당 메시지를 받아와 처리하면 될 것이다.

먼저 /grid_map_visualization/elevation_grid  메시지의 타입을 확인해야 한다.

해당 메시지의 타입은 nav_msgs/OccupancyGrid  이다.

그럼 Subscriber에서 해당 타입의 메시지를 불러올 수 있도록 설정하는데 이를 위해 opt/ros/kinetic/include 에 해당 라이브러리가 있는지 확인한다.

아래 그림에서 확인할 수 있듯이 nav_msgs/OccupancyGrid.h가 존재한다.

nav_msgs/OccupancyGrid 의 메시지 구성은 위의 우측에 나타낸 것과 같으며

따라서 나는 Octomap을 통해 작성한 2.5D map(Elevation map)이 잘 받아오는지 확인하기 위해

아래와 같이 코드를 구성했다.

#include<cv_bridge/cv_bridge.h>
#include<ros/ros.h>
#include<iostream>
#include<opencv2/opencv.hpp>
//#include<opencv2/highgui/highgui.hpp>
#include "nav_msgs/GetMap.h"
#include "image_transport/image_transport.h"
#include "grid_map_msgs/GridMap.h"
#include "nav_msgs/OccupancyGrid.h"
#include "std_msgs/Float32MultiArray.h"

cv::Mat input_map;
using namespace cv;


void mapConvert(const nav_msgs::OccupancyGrid& msg){
    try{
    std::cout<<"msg.MetaData: "<<msg.info<<std::endl;
    std::cout<<"msg.MetaData.width: "<<msg.info.width<<std::endl;
    std::cout<<"msg.MetaData.height: "<<msg.info.height<<std::endl;
    std::cout<<"msg.header: "<<msg.header<<std::endl;
    input_map = cv::Mat(msg.info.height, msg.info.width, CV_8UC1);

    for(int i=0; i<msg.info.height; i++){
        for(int j=0; j<msg.info.width; j++){
            int count = i*msg.info.width + j;
            if(msg.data[count] <= 0)
                input_map.at<uchar>(i, j) = 255;
            else{
                //std::cout<<"     valid data: "<<int(msg.data[count]);
                input_map.at<uchar>(i, j) = msg.data[count];
            }
        }
    }
    }
    catch (cv_bridge::Exception& e)
  {
    ROS_ERROR("Could not convert from");
  }
}

int main(int argc, char** argv){

    ros::init(argc, argv, "opencv_version_test");
    ros::NodeHandle nh;
    std::cout<<"OpenCV Version: "<<CV_VERSION<<std::endl;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher pub = it.advertise("AA/image", 1);

    //sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", input_map).toImageMsg();

    ros::Subscriber sub = nh.subscribe("/grid_map_visualization/elevation_grid", 1000, mapConvert);
    //ros::spin();

    cv_bridge::CvImagePtr cv_ptr(new cv_bridge::CvImage);

    ros::Rate loop_rate(5);
    while(nh.ok()){
        ros::spinOnce();
        cv::equalizeHist(input_map, input_map);
        if(input_map.cols != 0 && input_map.rows!=0){
            imshow("view", input_map);
            waitKey(5);
        }
        cv_ptr->encoding = "mono8";
        cv_ptr->header.stamp = ros::Time::now();
        cv_ptr->header.frame_id = "/AAAID";
        cv_ptr->image = input_map;
        pub.publish(cv_ptr->toImageMsg());
        
        // sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", input_map).toImageMsg();
        // pub.publish(img_msg);
        loop_rate.sleep();
    }
    return 0;
}

코드의 16~40 line은 콜백함수를 처리하는 부분으로 line52를 통해 지정한 Subscriber를 통해 작업을 수행하는 부분이다.

즉 line58의 while문을 통해 subscribe하기로 지정한 /grid_map_visualization/elevation_grid 메시지를 전달받게 되면 line16~40의 부분이 실행되고 결과적으로 전역변수로 선언된 Mat::input_map 변수를 정의하게 된다.

이 과정까지가 line59의 spinOnce()를 통해 이루어지는 과정이고 이후 전달받은 input_map을 통해 이를 imshow()로 시각화해주며 cv_ptr을 통해 RVIZ에서 확인할 수 있는 이미지 메시지로 변환하여 publish하게 된다.

위의 사진은 RVIZ에서 취득되는 2.5차원 지도를 Image 형태로 다시 Publish하는 결과를 나타낸다.

ROS에서 서비스 통신은 아래와 같은 시스템을 갖는다.

즉 서버는 항상 유지된 상태로 Client의 요청이 있으면 이에 대한 응답을 진행하는 동기(Syncronized)방식으로 구성된다.

이를 위해선 Client의 요청을 기다리는 Server Node와 해당 서버노드에 정보를 요청하는 Client Node가 필요하다.

따라서 본 게시물에서는 ROS환경에서 Server Node와 Client Node를 생성하여 실행시켜 본다.

우선 catkin_ws 이름의 디렉토리를 생성하고 해당 디렉토리 내에 src 디렉토리를 생성한다.

$ mkdir catkin_ws  && cd catkin_ws && mkdir src

그리고 src경로로 이동 후 아래 명령어를 입력하여 패키지를 생성한다.

$ cd catkin_ws/src

$ catkin_create_pkg beginner_tutorials rospy roscpp

여기까지 되었으면 한번 catkin_make를 통해 빌드해주고 source 해주자.

빌드 후 catkin_ws/src/beginner_tutorials 경로에 srv 디렉토리를 하나 생성한 후 아래와 같이 AddTwoInts.srv 파일을 생성한다.

<AddTwoInts.srv>

int64 a
int64 b
---
int64 sum

여기서 int64로 선언된 a와 b는 Client에서 Request할때 서버로 전달 될 파라미터를 정의하는 부분이고

sum은 서버에서 a와 b를 통해 작업을 수행한 결과를 Client로 Response 해주기 위한 부분으로 볼 수 있다.

여기서 Request로 정의된 변수들과 Response로 정의된 변수들은 "---" 로 구분짓게 된다.

그럼 이제 본격적으로 Client와 Server Node에 대한 source code를 작성해보자.

이는 catkin_ws/src/beginner_tutorials/src 폴더에 add_two_client.cpp 와  add_two_server.cpp 파일로 작성한다.

//// add_two_client.cpp 파일의 code

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <cstdlib>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_client");
  if (argc != 3)
  {
    ROS_INFO("usage: add_two_ints_client X Y");
    return 1;
  }

  ros::NodeHandle n;
  ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
  beginner_tutorials::AddTwoInts srv;
  srv.request.a = atoll(argv[1]);
  srv.request.b = atoll(argv[2]);
  if (client.call(srv))
  {
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);
  }
  else
  {
    ROS_ERROR("Failed to call service add_two_ints");
    return 1;
  }

  return 0;
}

//// add_two_server.cpp 파일의 code

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"

bool add(beginner_tutorials::AddTwoInts::Request  &req,
         beginner_tutorials::AddTwoInts::Response &res)
{
  res.sum = req.a + req.b;
  ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
  ROS_INFO("sending back response: [%ld]", (long int)res.sum);
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "add_two_ints_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("add_two_ints", add);
  ROS_INFO("Ready to add two ints.");
  ros::spin();

  return 0;
}

src 파일의 추가가 완료되었으면 이를 빌드해주기 위한 CMakeLists.txt 파일의 내용을 수정해주어야 한다.

CMakeLists.txt 파일에 아래 내용을 추가하자.

find_package(catkin REQUIRED COMPONENTS
    message_generation
)

add_service_files(
    FILES
    AddTwoInts.srv
)

catkin_package(
    CATKIN_DEPENDS message_runtime
)

여기서 AddTwoInts.srv 는 앞에서 작성한 .srv 파일의 이름을 넣어준다.

그리고 이제 마지막으로 노드를 실행하기 위해 실행가능한 노드의 이름과 source 코드를 지정해주는 부분이다.

add_executable(add_two_ints_server src/add_two_server.cpp)
target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
add_dependencies(add_two_ints_server beginner_tutorials_gencpp)

add_executable(add_two_ints_client src/add_two_client.cpp)
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
add_dependencies(add_two_ints_client beginner_tutorials_gencpp)

여기까지 진행되었으면 다시 catkin_make를 통해 빌드를 진행하고 source 해준다.

이제 server node와 client node를 실행시켜보자.

$ roscore

$ rosrun beginner_tutorials add_two_ints_server

$ rosrun beginner_tutorials add_two_ints_client 534 124

위의 명령어를 차례대로 입력하면

add_two_ints_server 노드가 먼저 실행되면서 Client의 Request를 기다리는 상황이 되고

add_two_ints_client 노드가 534, 124의 값을 서버로 전달함으로서 서버에서는

Request받은 534, 124의 값을 통해 연산을 진행한 후 그 결과(sum)를 반환(response)한다.

그리고 이떄 server_node.cpp 에서

ros::spin()  의 함수 대신

while(1){

  ros::spinOnce();

  ros::Duration(1.0).sleep();

}

위의 문장으로 바꿔봤는데 이때에는 client의 요청을 1초에 하나씩 받아서 처리된다.

즉 clinet에서 request값을 받은 후 서버로 요청하여 client.call(srv) 과정에 멈춰있다.

그리고 server로 부터 응답을 받으면 바로 client.call(srv) 후의 과정을 진행하게 된다.

즉, 클라이언트에서 아무리 빨리 정보를 request 하더라도 서버에서 spinOnce()를 사용할 경우

한번의 callback만 처리하므로 서버의 응답을 계속 대기하다가 응답을 받으면 다음 요청을 진행하게 된다.

즉 서버의 spinOnce() 가 실행되기 이전에 client에서 요청이 있었으면 processing을 진행하여 client로 reponse를 해준다.

client에서 request  ->  spinOnce()   ->   server processing   ->    server response  ->   client got response  의 흐름이다.

그럼 서버에서 데이터 처리 후 1.0초의 딜레이를 갖는데 서버의 함수에서 1.5초의 딜레이를 갖게되면 어떻게 될까?

서버에서는 client의 request를 확인하고 1.5초동안 함수에서 진행 후 빠져나와 1.0초를 대기하고 다시 client의 request를 확인한다.

즉, spinOnce 함수를 사용하면 서버의 processing이 모두 완료 되어야지만 다음 request를 진행할 수 있다.

서버도 request받은 내용을 모두 처리한 후 response 해야지만 다음 request를 받을 수 있다.