[KUSMO] 6/4 Arduino Symbolic Link 생성, laser_filters를 통한 프로파일 제거

• 로봇이 목적지에 도착했음을 인지 후 접속부를 구동시킬 방안 탐색
  • 정밀도 향상을 위해 목적지를 2개 이상으로 부여할 계획인데, 이 경우 위의 방식이 부적합할 수도 있음
• Move Base node가 publish하는 '/move_base/result/' topic을 통해 목적지 도착 여부를 알 수 있음
  • 'MoveBaseActionResult' message의 'status' field를 확인하여 로봇이 목적지에 도착했는지 여부를 알 수 있음
  • Navigation Goal Status를 포함
• 목적지에 도착했을 경우 아래의 값이 발행됨

 

#include <ros.h>
#include <move_base_msgs/MoveBaseActionResult.h>

ros::NodeHandle nh;
ros::Subscriber<move_base_msgs::MoveBaseActionResult> goalResultSub("move_base/result", goalResultCallback);

void setup() {
  nh.initNode();
  nh.subscribe(goalResultSub);
}

void loop() {
  nh.spinOnce();
}

void goalResultCallback(const move_base_msgs::MoveBaseActionResult& msg) {
  if (msg.status.status == actionlib_msgs::GoalStatus::SUCCEEDED) {
    // Activate actuators here (e.g., send a command to the Arduino)
    // Example: Serial.println("Goal reached!");
  }
  else {
    // Goal not reached
  }
}

• 목적지 도착 후의 동작을 설계하기 위한 코드

 

• Arduino 연결을 위한 Symbolic Link 생성

 

/* 
 This code Controls the Connection`s two Actuators by Keyboard input 
 'q', 'w', 'e' for Z Actuator,
 'a', 's', 'd' for Y Actuator 
*/

#include <Stepper.h>        // Library to use Stepper Motor

#define EN 8
const int stepPin = 2;      // X.SETP Motor's direction control. Motor moves one step by pulse sent to this pin
const int dirPin = 5;       // X.DIR Motor's Rotation Direction Control
const int degree_per_revolition = 200;

// Y Direction Acutator Connection
const int enA = 52;         // Motor`s PWM Input Pin. Set the motor`s speed by this pin`s value
const int in1 = 50;         // Set Motor`s Direction
const int in2 = 48;         // Set Motor`s Direction
bool motorRunning = false;  // Variable to track if the motor is currently running

void setup() {
  Serial.begin(9600);
  // Set all pins to Output related to Z Actuator
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(EN, OUTPUT);
  digitalWrite(EN, LOW);

  // Set all pins to Output related to Y Actuator
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(enA, OUTPUT);
  digitalWrite(enA, 255);    // Set Y Actuator`s Speed
}

void loop() {
  if (Serial.available()) {         // Set to read Keyboard Input
    char input = Serial.read();
    Serial.print("Input : ");
    Serial.println(input);

    /* Z Actuator Control */
    if (input == 'q') {
      digitalWrite(dirPin, HIGH);   // Set Z Actuator to move Upward
      motorRunning = true;          // Start the motor
    }
    else if (input == 'e') {
      digitalWrite(dirPin, LOW);    // Set Z Actuator to move Downward
      motorRunning = true;          // Start the motor
    }
    else if (input == 'w') {
      motorRunning = false;         // Stop Z Actuator
    }

    /* Y Actuator Control */
    else if(input == 'a'){          // Set Y Actuator to move Forward
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
    }
    else if(input == 'd'){          // Set Y Actuator to move Backward
      digitalWrite(in1, LOW);
      digitalWrite(in2, HIGH);
    }
    else if(input == 's'){          // Stop Y Actuator
      digitalWrite(in1, LOW);
      digitalWrite(in2, LOW);
    }

    /* Stop all the Actuators */
    else if(input == 'x'){          
      motorRunning = false;   
      digitalWrite(in1, LOW);
      digitalWrite(in2, LOW);      
    }
  }

  /* Set Z Actuator */
  if (motorRunning) {
    // 1.8 degrees per 1 revolution
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800);         // Set Z Actuator`s speed
    digitalWrite(stepPin, LOW);
    delayMicroseconds(800);
  }

}

• 키보드 입력을 통해 접속부의 두 액추에이터를 동시 구동할 코드 작성
• CNC Shield가 Arduino의 모든 GND 핀을 사용하여 L298N과 GND연결을 할 핀이 없음 : 대책 모색 필요
  • Arduino 2개 사용?

 

Lidar가 주변의 프로파일을 장애물로 인식함

 

• Lidar가 주변의 프로파일들을 장애물로 인식하는 문제를 해결하기 위해 laser_filters의 'LaserScanAngularBoundsFilterInPlace'를 통해 Lidar가 프로파일 4개를 무시하게 함
  • Lidar 기준으로 4개의 프로파일들의 각도를 측정 후 해당 각도 제거
  • LaserScanAngularBoundsFilterInPlace : 특정 각도내의 point들을 지움

 

scan_filter_chain:
- name: ProfileFrontLeft
  type: laser_filters/LaserScanAngularBoundsFilterInPlace
  params:
    lower_angle: 0.838     # 48.0138
    upper_angle: 1.038     # 59.4730

- name: ProfileBackLeft
  type: laser_filters/LaserScanAngularBoundsFilterInPlace
  params:
    lower_angle: 2.253    # 129.0873
    upper_angle: 2.451    # 140.4319    

- name: ProfileBackRight
  type: laser_filters/LaserScanAngularBoundsFilterInPlace
  params:
    lower_angle: -2.402    # 222.4756
    upper_angle: -2.215    # 233.0009

- name: ProfileFrontRight
  type: laser_filters/LaserScanAngularBoundsFilterInPlace
  params:
    lower_angle: -0.982    # 303.7356
    upper_angle: -0.778    # 315.4239

 

laser_filters 사용 전/후

 

 

• Arduino를 통한 FSR 압력센서 값 수신

 

 

---

참고 자료 :

https://kaiyuzheng.me/documents/navguide.pdf

 

https://pinkwink.kr/1302

ROS 라이다 scan 신호를 필터링하고 싶을때 laser filters pkg

 

http://wiki.ros.org/move_base

move_base - ROS Wiki

 

https://jkros.org/

https://jkros.org/

 

https://m.blog.naver.com/boilmint7/221924774050