("local_sys", 100);
+
+ while(ros::ok()){
+
+ ros::spinOnce();
+
+ sensor_msgs::PointCloud all_sensor; // 메세지 구조체 선언
+ all_sensor.points.resize(9); // Points 개수는 총 9개
+
+ for(kk=0; kk < 9; kk++){ // 반복문을 통해 센서 9개 값 메세지에 저장
+ all_sensor.points[kk].x = hall_[kk].x_val_real;
+ all_sensor.points[kk].y = hall_[kk].y_val_real;
+ all_sensor.points[kk].z = hall_[kk].z_val_real;
+ }
+
+ pub_all.publish(all_sensor); // 최종적으로 publish
+
+ }
+
+
+
+ return 0;
+}
+
+
+
+
+
+
+
+
+
+
+/////////////////
+// 함수 정의 구간 //
+/////////////////
+
+/*
+// 몇 번 센서 값을 출력해서 볼지 결정해주는 변수 값 메세지 입력받는 함수
+void modeCallback(const std_msgs::String::ConstPtr& msg)
+{
+ sensor_num_char = msg->data[0]; // sensor_select node에서 해당 값을 입력받음
+ sensor_num_int = sensor_num_char - 48; // 사용하기 편하게 정수형으로 바꿔서 저장
+}
+*/
+
+
+// Rosserial을 통해 subscribe하는 값을 적절히 받아서 완전한 형태의 패킷으로 정리 & 배열로 다시 저장하는 함수
+void chatterCallback(const std_msgs::String::ConstPtr& msg)
+{
+ // 32개씩 10개의 문자열이 들어오므로, Z가 검출될 때를 기준으로 10번 버퍼에 담으면 된다.
+ if(start_flag == 0)
+ if(msg->data[0] == 'Z') start_flag = 1; // 1. 수신받은 문자열 첫 문자가 Z로 시작하면 start_flag 1로
+
+
+ // 2. start_flag 값이 1이면
+ if(start_flag == 1) {
+ for(int k=0; k < 32; k++)
+ incomingData[i++] = msg->data[k]; // 3. 들어오는 문자열 이어서 버퍼에 저장
+
+ count_num++; // 분할 패킷을 총 몇 번 받았는지 카운트
+
+ if(count_num == 10) start_flag = 2; // 분할 패킷을 총 10번 받았으면 완성된 것이므로 다음 작업으로
+ }
+
+
+ if(start_flag == 2){
+ i = 3; // 6. 인덱스 초기화
+ start_flag = 0; // 7. 버퍼에 통신 값 저장을 알리는 플래그 변수도 0으로
+ count_num = 0;
+ mode = 1; // 8. 센서 값 분리 모드 On
+ working_func(); // 9. 센서 값 분리하는 함수 호출 -> 본격적인 작업 시작
+ }
+
+}
+
+
+
+// 본격적으로 센서 값의 분리 작업을 진행하는 함수
+void working_func(){
+
+ if (mode == 1) {
+ printf("%s\n", incomingData); // 문자열로 출력
+ // 헤더 분리
+ //for (kk = 0; kk < 3; kk++) header[kk] = incomingData[i++];
+
+ // 센서마다 값 분리
+ for (kk = 0; kk < 9; kk++) {
+ hall_buffer_func();
+ if (print_check_mode == 1) printf("%s\n", hall_[kk].full_val);
+ }
+
+ // checksum 값 분리
+ for (k = 0; k < 9; k++) { // checksum 값 10개만 딱 받기
+ checkSum_arr[k] = incomingData[i++]; // 차례로 checksum 변수에 저장
+ }
+ k = 0; // 다른 곳에서 사용될 것을 감안하여 초기화를 해 준다
+
+ mode++; // 다음 작업으로
+
+ }
+
+
+ // mode = 2일때: 각 센서값을 x, y, z값으로 분해 후 정수형으로 변환
+ if (mode == 2) {
+
+ // 축 별로 값 분리 후 정수형으로 변환
+ // if (print_check_mode == 1) printf("------------------------------------\n");
+ for (kk = 0; kk < 9; kk++) {
+ seperate_axis_val();
+ //if (print_check_mode == 1) printf("%d %d %d\n", hall_[kk].x_val_int, hall_[kk].y_val_int, hall_[kk].z_val_int);
+
+ }
+ //printf("process complete4\n");
+
+ // checksum 정수형으로 변환
+ int sign = 0;
+ if (checkSum_arr[0] == '1') { checkSum_arr[0] = '0'; sign = -1; }
+ else sign = 1;
+ checkSum = custom_atoi(checkSum_arr) * sign;
+
+ // 지금까지 분리했던 모든 센서 값들 전부 더하기
+ for (kk = 0; kk < 9; kk++)
+ checkSum_here += (hall_[kk].x_val_int) + (hall_[kk].y_val_int) + (hall_[kk].z_val_int);
+
+
+ //if (print_check_mode == 1) printf("%d, %d\n", checkSum, checkSum_here);
+
+ // checksum 비교
+ if (checkSum == checkSum_here) {
+ for (kk = 0; kk < 9; kk++) turn2real();
+ //printf("Packet Comm Success!!\n");
+ //printf("x: %f | y: %f | z: %f | count: %d %d\n", hall_[sensor_num_int].x_val_real, hall_[sensor_num_int].y_val_real, hall_[sensor_num_int].z_val_real, timerr++, sensor_num_int);
+ allPrint(); // 센서 값 출력
+ }
+ else {
+ //printf("Try again...\n");
+ //mode = 2;
+ } // 그러나 일치하지 않으면 다시 처음부터
+
+ reset_func(); // 한 번 통신이 끝나면 지금까지 사용했던 변수들 전부 초기화
+ }
+
+
+}
+
+
+
+
+// n번째 센서 값 별로 분리 후 저장해주는 함수
+void hall_buffer_func() {
+
+ for (kk = 0; kk < 9; kk++) {
+ for (int iii=0; iii < 29; iii++) // 딱 29개의 문자를 저장
+ hall_[kk].full_val[iii] = incomingData[i++]; // 센서값 통째로 버퍼에 저장
+
+ i = i+2; // 중간중간 껴 있는 알파벳 무시하고 바로 다음 센서 값으로
+ }
+ i--; // 센서 값 이후 index를 Checksum에 위치시키기 위한 조절
+}
+
+
+
+// n번째 센서 값을 x, y, z축별로 세부 분리 후 정수형으로 따로 저장해주는 함수
+void seperate_axis_val(){
+
+ int sign = 0; // 센서 값의 부호를 결정하기 위한 부호 변수(정수)
+ k = 0;
+
+ for(kk = 0; kk <9; kk++){ // n번째 센서 선택
+
+ for(l = 0; l < 9; l++) hall_[kk].x_val[l] = hall_[kk].full_val[k++]; // n번째 센서 값의 x값 추출
+ if (print_check_mode == 1) std::cout << hall_[kk].x_val;
+ if (print_check_mode == 1) std::cout << " ";
+ if (hall_[kk].x_val[0] == '1') { hall_[kk].x_val[0] = '0'; sign = -1; } // 패킷 맨 앞자리가 1이면 음수이므로, 0으로 바꾼 후 음수 연산자 설정
+ else sign = 1; // 그런데 0으로 시작하면 양수이므로, 양수 연산자로 설정
+ hall_[kk].x_val_int = custom_atoi(hall_[kk].x_val) * sign; // 추출한 값을 정수형으로 변환 + 부호 복원
+
+ k++; // 콤마 패스
+
+ for(l = 0; l < 9; l++) hall_[kk].y_val[l] = hall_[kk].full_val[k++]; // n번째 센서 값의 y값 추출
+ if (print_check_mode == 1) std::cout << hall_[kk].y_val;
+ if (print_check_mode == 1) std::cout << " ";
+ if (hall_[kk].y_val[0] == '1') { hall_[kk].y_val[0] = '0'; sign = -1; }
+ else sign = 1;
+ hall_[kk].y_val_int = custom_atoi(hall_[kk].y_val) * sign;
+
+ k++; // 콤마 패스
+
+ for(l = 0; l < 9; l++) hall_[kk].z_val[l] = hall_[kk].full_val[k++]; // n번째 센서 값의 z값 추출
+ if (print_check_mode == 1) std::cout << hall_[kk].z_val ;
+ if (print_check_mode == 1) std::cout << "\n\n";
+ if (hall_[kk].z_val[0] == '1') { hall_[kk].z_val[0] = '0'; sign = -1; }
+ else sign = 1;
+ hall_[kk].z_val_int = custom_atoi(hall_[kk].z_val) * sign;
+
+ k = 0;
+ l = 0; // n+1번째 센서 값의 세부 값 추출을 위해 인덱스 변수 초기화
+
+ }
+
+}
+
+
+
+
+// 모든 센서 값을 정수형에서 실수형으로 최종 변환해주는 함수
+void turn2real() {
+ for (kk = 0; kk < 9; kk++){
+ hall_[kk].x_val_real = (double)hall_[kk].x_val_int / 100 * (-1); // 왼손법칙 좌표계에서 오른손법칙 좌표계로 변환하기 위해 부호 변경
+ hall_[kk].y_val_real = (double)hall_[kk].y_val_int / 100; // 정수 변환 때는 checksum 계산 때문에 나중에 부호 변경을 해야 함
+ hall_[kk].z_val_real = (double)hall_[kk].z_val_int / 100;
+ }
+}
+
+
+
+// 모든 센서 값을 출력해주는 함수
+void allPrint() {
+ // printf("%d\n\n", timerr++);
+ for (kk = 0; kk < 9; kk++)
+ printf("%dth sensor x: %f | y: %f | z: %f | \n", kk+1, hall_[kk].x_val_real, hall_[kk].y_val_real, hall_[kk].z_val_real);
+ printf("\n\n");
+
+}
+
+
+
+// 패킷 분리작업 때 사용한 변수들 초기화하는 함수
+void reset_func() {
+ mode = 0;
+ //for (kk = 0; kk < 9; kk++) hall_[kk] = { 0, };
+ //for (kk = 0; kk < 4096; kk++) incomingData[kk] = { 0 };
+ //for (kk = 0; kk < 10; kk++) checkSum_arr[kk] = 0;
+ for (kk = 0; kk < 3; kk++) header[kk] = 0;
+ i = 0;
+ k = 0;
+ l = 0;
+ kk = 0;
+ checkSum = 0;
+ checkSum_here = 0;
+
+
+ //large_cap = 'A';
+ //small_cap = 'a';
+
+}
+
+
+
+// atoi 함수 구현
+int custom_atoi(const char *str)
+{
+ long long int sign;
+ long long int value;
+
+ sign = 1;
+ value = 0;
+ while (*str == '\t' || *str == '\n' || *str == '\v' \
+ || *str == '\f' || *str == '\r' || *str == ' ')
+ str++;
+ if (*str == '+' || *str == '-')
+ {
+ if (*str == '-')
+ sign *= -1;
+ str++;
+ }
+ while (*str >= 48 && *str <= 57)
+ {
+ value *= 10;
+ value += *str - 48;
+ str++;
+ }
+ return (value * sign);
+}
\ No newline at end of file
diff --git a/src/serial-example/src/serial_example_node.cpp b/src/serial-example/src/serial_example_node.cpp
new file mode 100644
index 0000000..4911183
--- /dev/null
+++ b/src/serial-example/src/serial_example_node.cpp
@@ -0,0 +1,74 @@
+/***
+ * This example expects the serial port has a loopback on it.
+ *
+ * Alternatively, you could use an Arduino:
+ *
+ *
+ * void setup() {
+ * Serial.begin();
+ * }
+ *
+ * void loop() {
+ * if (Serial.available()) {
+ * Serial.write(Serial.read());
+ * }
+ * }
+ *
+ */
+
+#include
+#include
+#include
+#include
+
+serial::Serial ser;
+
+void write_callback(const std_msgs::String::ConstPtr& msg){
+ ROS_INFO_STREAM("Writing to serial port" << msg->data);
+ ser.write(msg->data);
+}
+
+int main (int argc, char** argv){
+ ros::init(argc, argv, "serial_example_node");
+ ros::NodeHandle nh;
+
+ ros::Subscriber write_sub = nh.subscribe("write", 1000, write_callback);
+ ros::Publisher read_pub = nh.advertise("read", 1000);
+
+ try
+ {
+ ser.setPort("/dev/ttyUSB0");
+ serial::Timeout to = serial::Timeout::simpleTimeout(1000);
+ ser.setBaudrate(115200);
+ ser.setTimeout(to);
+ ser.open();
+ }
+ catch (serial::IOException& e)
+ {
+ ROS_ERROR_STREAM("Unable to open port ");
+ return -1;
+ }
+
+ if(ser.isOpen()){
+ ROS_INFO_STREAM("Serial Port initialized");
+ }else{
+ return -1;
+ }
+
+ ros::Rate loop_rate(1000); // comm velocity
+ while(ros::ok()){
+
+ ros::spinOnce();
+
+ if(ser.available()){
+ //ROS_INFO_STREAM("Reading from serial port");
+ std_msgs::String result;
+ result.data = ser.read(ser.available());
+ //ROS_INFO_STREAM("Read: " << result.data);
+ ROS_INFO_STREAM(result.data); // print comm results
+ read_pub.publish(result);
+ }
+ loop_rate.sleep();
+
+ }
+}
diff --git a/src/serial_example_node.cpp b/src/serial_example_node.cpp
index fdd289c..4911183 100644
--- a/src/serial_example_node.cpp
+++ b/src/serial_example_node.cpp
@@ -24,7 +24,7 @@
serial::Serial ser;
void write_callback(const std_msgs::String::ConstPtr& msg){
- ROS_INFO_STREAM("Writing to serial port" << msg->data);
+ ROS_INFO_STREAM("Writing to serial port" << msg->data);
ser.write(msg->data);
}
@@ -37,9 +37,9 @@ int main (int argc, char** argv){
try
{
- ser.setPort("/dev/ttyACM0");
- ser.setBaudrate(9600);
+ ser.setPort("/dev/ttyUSB0");
serial::Timeout to = serial::Timeout::simpleTimeout(1000);
+ ser.setBaudrate(115200);
ser.setTimeout(to);
ser.open();
}
@@ -55,20 +55,20 @@ int main (int argc, char** argv){
return -1;
}
- ros::Rate loop_rate(5);
+ ros::Rate loop_rate(1000); // comm velocity
while(ros::ok()){
ros::spinOnce();
if(ser.available()){
- ROS_INFO_STREAM("Reading from serial port");
+ //ROS_INFO_STREAM("Reading from serial port");
std_msgs::String result;
result.data = ser.read(ser.available());
- ROS_INFO_STREAM("Read: " << result.data);
+ //ROS_INFO_STREAM("Read: " << result.data);
+ ROS_INFO_STREAM(result.data); // print comm results
read_pub.publish(result);
}
loop_rate.sleep();
}
}
-