2023. 5. 24. 16:01ㆍ카테고리 없음
roslaunch realsense2_camera rs_camera.launch filters:=pointcloud unite_imu_method:="linear_interpolation" enable_gyro:=true enable_accel:=true하고 rostopic list 하면
/camera/accel/imu_info
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions
/camera/color/image_raw/compressed/parameter_updates
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/compressedDepth/parameter_descriptions
/camera/color/image_raw/compressedDepth/parameter_updates
/camera/color/image_raw/theora
/camera/color/image_raw/theora/parameter_descriptions
/camera/color/image_raw/theora/parameter_updates
/camera/color/metadata
/camera/depth/camera_info
/camera/depth/color/points
/camera/depth/image_rect_raw
/camera/depth/image_rect_raw/compressed
/camera/depth/image_rect_raw/compressed/parameter_descriptions
/camera/depth/image_rect_raw/compressed/parameter_updates
/camera/depth/image_rect_raw/compressedDepth
/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/camera/depth/image_rect_raw/theora
/camera/depth/image_rect_raw/theora/parameter_descriptions
/camera/depth/image_rect_raw/theora/parameter_updates
/camera/depth/metadata
/camera/extrinsics/depth_to_color
/camera/gyro/imu_info
/camera/imu
/camera/motion_module/parameter_descriptions
/camera/motion_module/parameter_updates
/camera/pointcloud/parameter_descriptions
/camera/pointcloud/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/clicked_point
/diagnostics
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg
/tf
/tf_static
가 나옴.
다른 터미널에서 RVIZ 실행 하기
왼쪽 아래 Add 누르고 Image 선택. Image Topic에서 /camera/color/image_raw 를 선택하면 이미지가 보인다.
자료형 : sensor_msgs/Image
/camera/imu 토픽을 rostopic echo 하면 값이 보인다.
자료형 : sensor_msgs/Imu
header:
seq: 331
stamp:
secs: 1684912047
nsecs: 335985899
frame_id: "camera_imu_optical_frame"
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
x: -0.005235987715423107
y: 0.0
z: -0.005235987715423107
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration:
x: -0.8727918267250061
y: -9.610516548156738
z: 0.19551852127035205
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]가속도, 각속도가 나오긴 한다.
Add에서 PointCloud2 선택, /camera/depth/color/points 선택.
자료형 : sensor_msgs/PointCloud2
---------------------------------------
roscoreroslaunch realsense2_camera rs_camera.launch filters:=pointcloud unite_imu_method:="linear_interpolation" enable_gyro:=true enable_accel:=true
rosrun cv_camera cv_camera_node<< 이를 시행하면, 해당 카메라의 이미지를 ROS이미지 메시지로 publish 하여 다른 노드들이 subscribe 가능하게함.
이거는 pc 웹캠임.
roslaunch realsense2_camera rs_camera.launch filters:=pointcloud unite_imu_method:="linear_interpolation" enable_gyro:=true enable_accel:=true이거를 시행하면,
이렇게 나온다. 이건 realsense camera이다.
위의 둘 중의 한 방법을 사용하고,
roslaunch opencv_apps edge_detection.launch image:=/camera/color/image_raw를 하면
이런게 보인다.
edge_detection이라는 node가 /edge_detection/image 라는 topic을 publish하고 이는 sensor_msgs/Image 이다.
Edgy detection은 컴퓨터 비전 및 이미지 처리에서 사용되는 기술 중 하나입니다. 이 기술은 이미지에서 에지(Edge) 또는 경계선을 감지하는 것을 목표로 합니다.
위에서
roslaunch realsense2_camera rs_camera.launch filters:=pointcloud unite_imu_method:="linear_interpolation" enable_gyro:=true enable_accel:=true가 실행된 상태에서,
이번에는
roslaunch opencv_apps hough_lines.launch image:=/camera/color/image_raw를 실행하면
이렇게 나온다.
hough_lines 라는 node가
/hough_lines/Image라는 topic을 publish하고 이는 sensor_msgs/Image 자료형이다.
/hough_lines/Image 를 subscribe 받아서 직선상의 두 점을 추출하려고 했다.
수평선이 0도, 시계방향이 (+) 방향인 것 같다.
아래는 0도와 45도 사이일때만 터미널에 출력되도록 함
카메라의 높이가 낮으면 복도의 선같은것도 인식하기에 적당히 높이가 있어야 할듯.
------------
카메라 캘리브레이션
http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration
camera_calibration/Tutorials/MonocularCalibration - ROS Wiki
Note: This tutorial assumes that you have completed the previous tutorials: ROS Tutorials. Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of y
wiki.ros.org
IMU 캘리브레이션
python말고 python3로 실행하면 되는듯
{
"device_type": "D435i",
"imus": [
{
"accelerometer": {
"bias": [
0.07594006255915514,
-0.04151784897136185,
0.16189462951611122
],
"scale_and_alignment": [
1.0222701357183328,
0.013061016011728453,
-0.018177262375206837,
-0.012244353974671485,
1.0128684362431686,
0.010191526791643257,
-0.01680486575494662,
0.013256825222412671,
1.0178986863833155
]
},
"gyroscope": {
"bias": [
-0.005753931074313889,
-0.0013410534236411617,
-0.004029319597420725
],
"scale_and_alignment": [
1.0,
0.0,
0.0,
0.0,
1.0,
0.0,
0.0,
0.0,
1.0
]
}
}
]
}
depth 캘리브레이션
realsense-viewer 실행 후
On-Chip Calibration 실행.
이거누르고 픽셀이 좀 많아야 되는듯. 여기저기 비춰준다.
-------------------------
opencv_apps/LineArryStamped 자료형의
/hough_lines/lines topic을 opencv_apps의 hough_lines.launch에서 publish하고,
rosrun hough_lines_output output_listener으로 subscribe 받아서
lineArrayCallback이라는 함수가 실행되면 여기서 각 점들을 구하고,
calculateIntersection이라는 함수에서 직선을 만들어 수평,평행을 체크하고 기울기가 10도와 80도 사이에 있는 필터를 거쳐서
교차점을 구한 뒤
filteredIntersection이라는 함수에서 x는 540부터 740, y는 400부터 600까지 boundary를 설정하고, 100개의 점을 누적하고
평균을 구한뒤 표준편차 1000을 설정하고
최종적으로 교차점을 구했다.
#include <ros/ros.h>
#include <opencv_apps/LineArrayStamped.h>
#include <geometry_msgs/Point.h>
#include <vector>
#include <sensor_msgs/Image.h>
#include <deque>
#include <algorithm>
#include <numeric>
#include <cmath>
#define rad2deg(radians) ((radians) * 180.0 / M_PI)
// Maximum number of points to keep in memory
#define MAX_POINTS 100
// Deque to hold intersection points
std::deque<geometry_msgs::Point> intersectionPoints;
// Your desired range
// resolution: 1280x720
// middle point: (640, 360)
double xMin = 540;
double xMax = 740;
double yMin = 400;
double yMax = 600;
double min_angle = 10;
double max_angle = 80;
// Your desired standard deviation
double desiredStdDev = 1000;
// Filter intersection
void filteredIntersection(const geometry_msgs::Point& point)
{
// Your intersection calculation code goes here...
// Let's say intersection point is (xIntersection, yIntersection)
// Area based filtering
if (point.x < xMin || point.x > xMax || point.y < yMin || point.y > yMax) {
// ROS_WARN("Intersection point out of range. Skipping...");
return;
}
// Create a geometry_msgs::Point for the intersection point
geometry_msgs::Point intersection;
intersection.x = point.x;
intersection.y = point.y;
intersection.z = 0.0;
// Add the intersection point to the deque
intersectionPoints.push_back(intersection);
// Check if deque size exceeds MAX_POINTS
if (intersectionPoints.size() > MAX_POINTS) {
intersectionPoints.pop_front(); // Remove the oldest point
}
// Calculate mean
double meanX = std::accumulate(intersectionPoints.begin(), intersectionPoints.end(), 0.0,
[](double sum, const geometry_msgs::Point& pt) { return sum + pt.x; }) / intersectionPoints.size();
double meanY = std::accumulate(intersectionPoints.begin(), intersectionPoints.end(), 0.0,
[](double sum, const geometry_msgs::Point& pt) { return sum + pt.y; }) / intersectionPoints.size();
// Calculate standard deviation
double stdDevX = std::sqrt(std::accumulate(intersectionPoints.begin(), intersectionPoints.end(), 0.0,
[meanX](double sum, const geometry_msgs::Point& pt) { return sum + (pt.x - meanX) * (pt.x - meanX); }) / intersectionPoints.size());
double stdDevY = std::sqrt(std::accumulate(intersectionPoints.begin(), intersectionPoints.end(), 0.0,
[meanY](double sum, const geometry_msgs::Point& pt) { return sum + (pt.y - meanY) * (pt.y - meanY); }) / intersectionPoints.size());
if (stdDevX > desiredStdDev || stdDevY > desiredStdDev) {
ROS_WARN("Standard deviation exceeds desired value. Skipping...");
intersectionPoints.pop_back(); // Remove the latest point
return;
}
// Print the intersection point
ROS_INFO("Intersection: (%f, %f)", intersection.x, intersection.y);
}
// Calculate intersection of 4 points representing two lines
void calculateIntersection(const geometry_msgs::Point& pt1, const geometry_msgs::Point& pt2, const geometry_msgs::Point& pt3, const geometry_msgs::Point& pt4)
{
// Check if the lines are vertical
bool isVertical1 = (pt2.x - pt1.x) == 0;
bool isVertical2 = (pt4.x - pt3.x) == 0;
// Check if the lines are parallel
bool isParallel = ((pt2.y - pt1.y) / (pt2.x - pt1.x)) == ((pt4.y - pt3.y) / (pt4.x - pt3.x));
// Skip calculation if lines are vertical or parallel
if (isVertical1 || isVertical2 || isParallel)
{
// ROS_WARN("Lines are vertical or parallel. Skipping intersection calculation.");
return;
}
// Calculate the slopes of the two lines
double slope1 = (pt2.y - pt1.y) / (pt2.x - pt1.x);
double slope2 = (pt4.y - pt3.y) / (pt4.x - pt3.x);
// Define the threshold angle for excluding lines
double angleThreshold = 30.0; // Set your desired angle threshold here
// Calculate the angles of the two lines
double angle1 = atan(slope1) * 180.0 / M_PI;
double angle2 = atan(slope2) * 180.0 / M_PI;
if ( ( (angle1 > min_angle && angle1 < max_angle) || (angle1 < -min_angle && angle1 > -max_angle) ) && ( (angle2 > min_angle && angle2 < max_angle) || (angle2 < -min_angle && angle2 > -max_angle)) ) {
// Calculate the y-intercepts of the two lines
double yIntercept1 = pt1.y - slope1 * pt1.x;
double yIntercept2 = pt3.y - slope2 * pt3.x;
// Calculate the x-coordinate of the intersection point
double xIntersection = (yIntercept2 - yIntercept1) / (slope1 - slope2);
// Calculate the y-coordinate of the intersection point
double yIntersection = slope1 * xIntersection + yIntercept1;
// Create a geometry_msgs::Point for the intersection point
geometry_msgs::Point intersection;
intersection.x = xIntersection;
intersection.y = yIntersection;
intersection.z = 0.0;
// Print the intersection point
// ROS_INFO("Intersection: (%f, %f)", intersection.x, intersection.y);
// ROS_INFO("distance from center : (%lf, %lf)", intersection.x - 640, intersection.y - 360);
filteredIntersection(intersection);
}
}
void lineArrayCallback(const opencv_apps::LineArrayStamped::ConstPtr& msg)
{
// Check if the lines array is not empty
if (msg->lines.empty())
{
ROS_WARN("Empty lines array.");
return;
}
// Initialize lists to store pt1 and pt2 coordinates of each line
std::vector<geometry_msgs::Point> pt1_list;
std::vector<geometry_msgs::Point> pt2_list;
// Iterate over each line
for (const auto& line : msg->lines)
{
// Convert pt1 from opencv_apps::Point2D to geometry_msgs::Point
geometry_msgs::Point pt1;
pt1.x = line.pt1.x;
pt1.y = line.pt1.y;
pt1.z = 0.0; // Assuming z-coordinate is not provided in opencv_apps::Point2D
// Convert pt2 from opencv_apps::Point2D to geometry_msgs::Point
geometry_msgs::Point pt2;
pt2.x = line.pt2.x;
pt2.y = line.pt2.y;
pt2.z = 0.0; // Assuming z-coordinate is not provided in opencv_apps::Point2D
pt1_list.push_back(pt1);
pt2_list.push_back(pt2);
}
// Call calculateIntersection for each pair of lines
for (size_t i = 0; i < pt1_list.size(); ++i)
{
for (size_t j = i + 1; j < pt1_list.size(); ++j)
{
calculateIntersection(pt1_list[i], pt2_list[i], pt1_list[j], pt2_list[j]);
}
}
}
void imageCallback(const sensor_msgs::Image::ConstPtr& msg)
{
int width = msg->width;
int height = msg->height;
ROS_INFO("Image resolution: %dx%d", width, height);
// 1280x720
// 중심점 : 640x360
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "output_listener");
ros::NodeHandle nh;
// Subscribe to the LineArrayStamped topic
ros::Subscriber hough_line_sub = nh.subscribe("/hough_lines/lines", 1000, lineArrayCallback);
// ros::Subscriber image_sub = nh.subscribe("/camera/color/image_raw", 1000, imageCallback);
// Set the desired loop rate (e.g., 10 Hz)
ros::Rate loop_rate(10);
while (ros::ok())
{
ros::spinOnce(); // Process callback functions
// Your code here
loop_rate.sleep(); // Sleep to achieve the desired loop rate
}
return 0;
}
공학관 3층에서 실험한 결과는 다음과 같다.
안녕하세요! 제가 당신의 코드를 이해하는데 도움이 될 수 있도록 노력하겠습니다.
여기에 C++와 ROS(Robot Operating System)로 작성된 코드가 있습니다. 코드는 선(line) 배열을 받아 그 교차점을 계산하고 필터링하는 것을 목표로 합니다. 이 작업은 선 감지를 위한 이미지 처리 작업에 일반적으로 사용됩니다.
주요 요소는 다음과 같습니다:
calculateIntersection 함수: 4개의 점을 입력으로 받아 두 선의 교차점을 계산합니다. 두 선이 수직이거나 평행할 경우 교차점 계산은 건너뜁니다. 그렇지 않으면 두 선의 기울기와 y-절편을 계산하여 교차점을 찾습니다.
filteredIntersection 함수: 교차점을 입력으로 받아 특정 영역 및 표준 편차 내에 있는지 확인합니다. 해당 조건에 맞지 않는 교차점은 제거됩니다. 또한 이 함수는 최근 100개의 교차점만 유지하며, 더 많은 점이 추가되면 가장 오래된 점이 제거됩니다.
lineArrayCallback 함수: LineArrayStamped 메시지를 구독하여 받은 선 배열에서 모든 선 쌍의 교차점을 계산합니다.
imageCallback 함수: Image 메시지를 구독하여 이미지의 너비와 높이를 출력합니다. 현재는 주석 처리되어 있습니다.
main 함수: 노드를 초기화하고 /hough_lines/lines 및 /camera/color/image_raw 토픽을 구독합니다. 그리고 주 루프는 주어진 루프 속도를 유지하면서 콜백 함수를 처리합니다.
이것이 당신의 코드의 전반적인 개요입니다. 만약 더 깊이 파고들거나 특정 부분에 대해 논의하고 싶다면 알려주세요!
5월 28일
현재 intersection의 x좌표를 delivery_pkg에서 받아온다.
그 후 현재값을 교차점의 x좌표, 목표값을 640으로, p게인을 0.8로, d게인을 p게인의 5%로 설정함.
640픽셀이 690이나 590이 되면, 즉 50픽셀 이상 넘어가면 최대의 control_signal인 55 만큼 오른쪽 모터에 더하고 빼게 된다.
그러니까, 왼쪽 모터와 오른쪽 모터는 11만큼 차이가 난다. 244와 255.
지금은 189와 200을 기본으로 잡고, 오른쪽 모터가 200-55, 200+55 만큼 변하면서 직진을 유지하게 된다.
5월 29일
char room[] = "room_512"; // x와 y 좌표를 yaml파일에서 가져오고 싶은 방 호수 yaml.get_XY_Coordinates_From_yaml(room);
double x = yaml.get_x(room);
double y = yaml.get_y(room);이렇게 해서 yaml 파일 안에 있는 방 호수를 위에서 char 배열 변수로 지정하면 된다.
5월 30일
직진 시 왼쪽 195 오른쪽 200
배터리와 내 노트북 배터리 상태에 따라 왼쪽이 달라지는듯
topic 녹화하는 방법
rosbag record -O 0530_imageraw.bag /camera/color/image_rawrosbag play 녹화파일명.bag현재 p게인 1.1
아두이노 이 함수 사용 주의. 이게 추천되는 사용 방법이라고 공식 위키에 나와있음
attachInterrupt(digitalPinToInterrupt(21), doEncoderA_L, CHANGE);
5월 31일
오도메트리 구현.
엔코더 모터 홀 센서로 선속도 구하고 2로 나눠서 양쪽의 평균을 구해 x(로봇 전방) position 구하고,
d435i 내장 IMU로 degree(시계방향이 +)를 구함.
아두이노에서 왼쪽, 오른쪽 홀 센서를 로스 토픽으로 보냄
작성한 PDController 클래스 기중이 enco_odom 패키지가 아닌, delivery_pkg에 통합 작업을 함.
주요 함수
double PDController::calculate(double setpoint, double pv){
// Calculate error
error = setpoint - pv;
// Proportional term
P = Kp * error;
// Derivative term
derivative = (error - pre_error) * Kd;
// Calculate total output
output = P + derivative;
// Restrict to max/min
if (output < min) output = min;
else if (output > max) output = max;
// Save error to previous error
pre_error = error;
return output;
}
직진 시 오도메트리를 사용하기 위해, 즉 RobotController 클래스에서 위 calculate 함수를 사용하기 위해
PDController 클래스를 상속받음
class RobotController : public PDController다음과 같이 상속받으면, 이놈의 변수와 함수 모드 그냥 사용 가능. 클래스 객체 없이
그리고 두 클래스에서 모두 생성자를 사용하기에,
RobotController클래스의 생성자에서 PDController의 생성자를 명시적으로 호출해야 함.
RobotController::RobotController(ros::NodeHandle& nh) : PDController(nh), nh_(nh), loop_rate(LOOP_RATE)생성자 부분.
우측 통행 직진 pd 제어 실험
왼쪽 바퀴를 기준으로 오른쪽 벽으로부터 50cm 떨어진 상태에서 실험
setpoint 660 픽셀
직진 200cm
---
실험결과
직진거리 195
왼쪽바퀴부터 벽 60
카메라 690
왼쪽 206인데도 왼쪽으로 조금 감 더 올려야할듯
IMU직진이 너무 튄다. 오도메트리도 마찬가지로.
이유는 카메라가 복도의 라인을 잘 검출하도록 카메라의 오른쪽 방향의 축을 기준으로 10도 정도 양의 pitch 회전을 했기 때문.
그러니까 가민이 있어도 yaw 방향 각속도도 누적되고, 값이 너무 이상하다.
이를 보정하기 위해 계수를 곱해도 거의 매 실험마다, 노트북 usb 포트마다, 배터리, 발열 문제로 매번 계수를 곱해야 하는 상황이 발생함.
-> 그래서 그냥 이전에 하던 수치적인 실험적 방법(i 증가)로 직진과 회전을 구현하는 게 나을 듯 싶다.
여기에 직진 시에는 허프 변환, pd 제어가 들어간다.
결국 도돌이표인듯........
PD게인 그래프 한번 뽑아? rqt_graph?
복도 가운데 1000cm 직진 pd 제어 실험
직진성 검증 : 카메라 이미지의 중심인 640픽셀과 hough tranform으로 검출한 line들의 교차점의 x좌표의 차이를 그래프로 보자.
특정 토픽을 그래프로 볼 수 있다.
rosrun rqt_plot rqt_plot /x_point_data
이렇게 보니까 x축은 time step이 아니라 distance 인 것 같다.
640을 중심으로 진동하는 듯. 이걸 어떻게 풀어나갈지는 고민해야
직진주행 거리
149*7 + 32 = 1075.
오차 : 7.5%
simplescreenrecorder
60fps
------------------------------
6월 6일
tof_sensor라는 새로운 branch 만듬
tof로 3층에서 우측통행 실험
p게인 3, d게인 14, setpoint 25
60에서 튄 구간은 방문이 있는 부분이라 그럼
비교적 잘 수렴함