초음파 센서를 이용한 AEB 구현 1

초음파 센서를 이용한 AEB (Autonomous Emergency Brake) 구현

catkin_create_pkg aeb_control std_msgs rospy roscpp sensor_msgs geometry_msgs

pkg 이름 / standard message 사용 / Ultrasonic sensor 사용 / 차량 조종 cmd_vel 사용

 

---

Geany 사용하여 aeb_controller.cpp로 저장

 

Geany 실행 - untitled 상태 - File - Save as... - [ catkin_ws - src - aeb_control - src ] - " aeb_controller.cpp " - save

 

open - [ catkin_ws - src - aeb_control - CMakeLists.txt  ]

108 & 110 & 111 주석 # 지우기

 

 

129 부터 쭉 삭제 ( 안 씀 )

---

aeb_controller.cpp 파일

#include "ros/ros.h"
#include "std_msgs/Int8.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"	//cmd_vel

int main(int argc, char **argv)
{
	int count = 0;
	
	ros::init(argc, argv, "aeb_controller");

	ros::NodeHandle n;
	
	ros::Rate loop_rate(10); //10
	
	
	while (ros::ok())
	{
		loop_rate.sleep();
		ros::spinOnce();
		++count;
	}
	return 0;
}

---

cm = catkinworkspace로 이동, ROS 패키지 build

에러 발생

에러 원인 = cpp 파일에서 return을 reture로 오타 발생

해결 후 다시 cm, 성공.

확인차 rospack list, 등록 완료

= 성공적으로 환경설정 완료, 와^^

---

기능 추가

Range = 센서 거리 측정

 

#include sensor msgs/Range.h //ultrasonic sensor message

UltraSonicSansor 사용

+

void UltraSonarCallback 함수

Range 토픽 발생이 됨

(subscribe = Callback 함수에 연결)

 

http://wiki.ros.org/evarobot_sonar/Tutorials/indigo/Writing%20a%20Simple%20Subscriber%20for%20Sonar%20Sensor

evarobot_sonar/Tutorials/indigo/Writing a Simple Subscriber for Sonar Sensor - ROS Wiki

http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Range.html

sensor_msgs/Range Documentation

 

aeb_controller.cpp 파일 추가 내용

#include "ros/ros.h"
#include "std_msgs/Int8.h"
#include "std_msgs/String.h"
#include "sensor_msgs/Range.h"		//ultrasonic sensor message
#include "geometry_msgs/Twist.h"	//cmd_vel

void UltraSonarCallback(const sensor_msgs::Range::ConstPtr& msg)
{
	ROS_INFO("Sonar Seq: [%d]", msg->header.seq);
	ROS_INFO("Sonar Range: [%f]", msg->range);
}

int main(int argc, char **argv)
{
	int count = 0;
	
	ros::init(argc, argv, "aeb_controller");

	ros::NodeHandle n;
	
	ros::Rate loop_rate(10); //10
	
	ros::Subscriber sub = n.subscribe("range", 1000, UltraSonarCallback);
	
	
	while (ros::ok())
	{
		loop_rate.sleep();
		ros::spinOnce();
		++count;
	}
	return 0;
}

 

다시 cm

새 터미널 Ctrl + Shift + T

roscore - WARNING 발생

 

해결법 - 다른 창에서 rosclean purge - y = roscroe 안전 실행

 

 

rosrun aeb_control aeb_control_node

(잘 모르겠으면 CMackeList 로, 맨 위 패키지 이름, 맨 아래 노드 이름)

=아무것도 안 나타남, why? Topic이 날라오지 않아서.

새 터미널

rostopic list = range가 있지만 날라오는 Topic X

새 터미널, Gazebo 실행

cw

gz 

=> 실행되면서 퍼블리셔 진행, range data 나옴

움직여보면 Range 값 변화, 최대 감지 거리 2m

 

속도 0m/s로 바꿔도 바로 정지 안 됨

why? 차 관성. 시뮬레이션이 건드리기 때문

 

---

기능 추가

bool 기능 = aeb 작동 될 건지, 안 될 건지.

#include "ros/ros.h"
#include "std_msgs/Int8.h"
#include "std_msgs/Bool.h"			//boolean data
#include "std_msgs/String.h"
#include "sensor_msgs/Range.h"		//ultrasonic sensor message
#include "geometry_msgs/Twist.h"	//cmd_vel

std_msgs::Bool flag_AEB; //whether AEB is working

void UltraSonarCallback(const sensor_msgs::Range::ConstPtr& msg)
{
	ROS_INFO("Sonar Seq: [%d]", msg->header.seq);
	ROS_INFO("Sonar Range: [%f]", msg->range);
	
	if(msg->range <=1.0) //msg data less than 1m
	{
		ROS_INFO("AEB_Activation!!");
		flag_AEB.data = true; //put data
	}
	else //greater than 1m
	{
		flag_AEB.data = false; //not put data
	}
}

int main(int argc, char **argv)
{
	int count = 0;
	
	ros::init(argc, argv, "aeb_controller");

	ros::NodeHandle n;
	
	ros::Rate loop_rate(10); //10
	
	ros::Subscriber sub = n.subscribe("range", 1000, UltraSonarCallback);
	
	
	while (ros::ok())
	{
		loop_rate.sleep();
		ros::spinOnce();
		++count;
	}
	return 0;
}

 

roscore 제외 나머지 실행 중단, Ctrl + C → cm

다시 rosrun aeb_control aeb_control_node 실행

(아무것도 안 뜨면 Gazebo 재실행 "gz" ) 

 

if) 1m 이하면 AEB_Activation!! 뜸

---

rqt_graph

다음에는 /range 가 /aeb_controller를 거쳐서 나오게 진행

---

Gazebo, roscore 중단

 

launch 폴더 생성

[ catkin_ws - src - aeb_control - New Folder - "launch" ]

aeb_control launch 생성

[ Geany - new file - save as... - catkin_ws - src - aeb_control - launch - "aeb_control.launch"]

<launch>

<node pkg="aeb_control" type="aeb_control_node" name="abe_control_node"  />

</launch>

roslaunch aeb_control aeb_control.launch

=아무것도 안 나타남

why? output screen이 없기 때문

 

해결법

roslaunch aeb_control aeb_control.launch --screen   &&   gz (다른 터미널 창)

=출력 나옴