HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

500
Controllers/Packages/nova5_control/example/simple.cpp Normal file
View File

@@ -0,0 +1,500 @@
#include"nova5_control/nova5_modbus.h"
#include <termios.h>
#include <stdio.h>
char getKey()
{
struct termios oldt, newt;
char ch;
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
newt.c_lflag &= ~(ICANON | ECHO);
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
ch = getchar();
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
}
int main(int argc,char **argv)
{
ros::init(argc,argv,"EXAMPLE");
ros::NodeHandle nh("~");
std::string IP = "192.168.2.6";
int PORT = 502;
// Coordinates
std::vector<double> HOME = {-141.91,301.01,524.27,-179.4,-0.04,0.0};
std::vector<double> CENTER_P = {309.11,63.29,524.27,-179.4,-0.04,0.00};
std::vector<double> ORIGIN_COOR = {150.71,-435.9,524.27,-179.4,-0.04,-90.00};
std::vector<double> OK_COOR = {-141.91,301.01,150,-179.4,-0.04,0.0};
std::vector<double> NG_COOR = {-141.91,657.01,150,-179.4,-0.04,0.0};
std::vector<double> TEST_COOR = {250.71,-480.9,424.27,-179.4,-0.04,-90.00};
std::vector<double> STORE_COOR(6);
std::vector<double> Point(6);
std::vector<double> Point_current(6);
std::vector<double> Center_temp(6);
std::vector<double> Point_temp1(6);
std::vector<double> Point_temp2(6);
std::vector<double> Point_temp3(6);
uint16_t isCheck = 0;
int timeStamp_cam = 0;
static int count = 0, count_1 = 0;
static int center_count = 0;
static int get_point_current =0;
static int ob = 0;
static uint16_t speed = 0;
static int count_ok = 0;
static int count_ng = 0;
bool connect = false;
bool drag = false;
bool flag_home = false;
bool poweroff = false;
// Khởi tạo
std::shared_ptr<cnt_nova5> nova5_ptr_ = nullptr;
nova5_ptr_ = std::make_shared<cnt_nova5>(IP,PORT);
// Kết nối đến robot nova
connect = nova5_ptr_->nova_connect();
if(connect)
{
if(nova5_ptr_->nova_robotMode() == 3)
{
if(nova5_ptr_->nova_powerOn() == -1)
{
ROS_ERROR("Please check robot arm");
return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
}
}
// Sau khi kết nối thành công cần enable robot
nova5_ptr_->nova_enable();
// Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
nova5_ptr_->nova_startProgram();
}
else
{
// alarm = nova5_ptr_->nova_statusAlarm();
ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
for(int i=0; i<180U; ++i)
{
connect = nova5_ptr_->nova_connect();
if(connect || !ros::ok())
{
while(ros::ok())
{
double mode = nova5_ptr_->nova_robotMode();
if(nova5_ptr_->nova_statusAlarm() == 1 && mode == 9) ROS_ERROR("Kinematic ERROR! Please check robot!");
if(mode == 3)
{
if(nova5_ptr_->nova_powerOn() == -1)
{
ROS_ERROR("Please check robot arm");
return -1;// biến tạm để cho thấy robot bị lỗi, thay thành các biến phản hồi lỗi
}
}
if(mode == 4) break;
ros::Duration(5).sleep();
}
// Sau khi kết nối thành công cần enable robot
nova5_ptr_->nova_enable();
// Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
nova5_ptr_->nova_startProgram();
break;
}
ros::Duration(5).sleep();
}
}
ros::Rate loop_rate(100);
//===========================Vòng lặp chính========================//
//=================================================================//
//=================================================================//
while(ros::ok() && connect)
{
static int temp = 0;
static bool checkconnect = false;
checkconnect = nova5_ptr_->nova_checkConnected();
if(checkconnect)
{
double robotmode = nova5_ptr_->nova_robotMode();
//===Kiểm tra trạng thái robot===//
//===============================//
if( robotmode == 3)
{
poweroff = true;
// Power on robot
if(nova5_ptr_->nova_powerOn() == 1)
{
// Sau khi power on cần enable robot
nova5_ptr_->nova_enable();
nova5_ptr_->nova_startProgram();
}
}
else if(robotmode == 6)
{
ROS_WARN("Robot is in drag mode");
drag = true;
}
if(drag == true && robotmode == 5)
{
ROS_WARN("PRESS: n to continute");
ROS_WARN("PRESS: h to go home");
char key = getKey();
ROS_WARN("%d", key);
/*Kiểm tra nếu key = n hoặc N thì thực hiện lệnh tiếp tục*/
if(key == 110 || key == 78)
{
nova5_ptr_->nova_startProgram();
drag = false;
get_point_current = 0;
count = 0;
center_count = 0;
flag_home = false;
}
/*Kiểm tra nếu key = h hoặc H thì thực hiện lệnh về home */
else if(key == 104)
{
nova5_ptr_->nova_startProgram();
drag = false;
get_point_current = 0;
center_count = 0;
ROS_INFO("Robot is comming home...");
flag_home = true;
}
}
//===============================//
//===============================//
//===Chu trình gắp phôi và lưu trữ phôi===//
//========================================//
/* Bước 0 ve home */
if(center_count == 0)
{
if(get_point_current == 0)
{
//Đọc tọa độ robot hiện tại trong vòng 6s
nova5_ptr_->nova_getCoorcurrent(Point_current);
ROS_ERROR("x current: %f", Point_current[0]);
ROS_ERROR("y current: %f", Point_current[1]);
ROS_ERROR("z current: %f", Point_current[2]);
if(CENTER_P[2] > (Point_current[2] + 10))
{
Center_temp[0] = Point_current[0];
Center_temp[1] = Point_current[1];
Center_temp[2] = nova5_ptr_->nova_check_z(Point_current[0],Point_current[1],CENTER_P[2],0);
Center_temp[3] = Point_current[3];
Center_temp[4] = Point_current[4];
Center_temp[5] = Point_current[5];
}
else if(CENTER_P[2] < (Point_current[2] - 10))
{
Center_temp[0] = CENTER_P[0];
Center_temp[1] = CENTER_P[1];
Center_temp[2] = nova5_ptr_->nova_check_z(CENTER_P[0],CENTER_P[1],Point_current[2], 0);
Center_temp[3] = CENTER_P[3];
Center_temp[4] = CENTER_P[4];
Center_temp[5] = CENTER_P[5];
}
else if(CENTER_P[2] >= (Point_current[2] - 10) && CENTER_P[2] < (Point_current[2] + 10)) center_count = 1;
ROS_WARN("x : %f", Center_temp[0]);
ROS_WARN("y : %f", Center_temp[1]);
ROS_WARN("z : %f", Center_temp[2]);
get_point_current = 1;
}
if(center_count == 0)
{
if(nova5_ptr_->nova_movePoint(Center_temp,100,1)==1 && center_count == 0) center_count++;
}
}
/* Bước 1 */
else if(center_count == 1)
{
if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && center_count == 1)
{
center_count = 2;
}
}
if(flag_home && center_count == 2)
{
if(nova5_ptr_->nova_movePoint(HOME,100,1)==1 && center_count == 2)
{
ROS_INFO("Complete");
// nova5_ptr_->nova_stopProgram();
// nova5_ptr_->nova_close();
return 0;
}
}
/* Bước 2 di chuyển cánh tay đến vị trí chụp ảnh */
if(!flag_home && center_count == 2 && count == 0)
{
if(nova5_ptr_->nova_movePoint(ORIGIN_COOR,100,1)==1 && count == 0)
{
count++ ;
}
}
/* Bước 3 xác định vị trí vật thể và di chuyển tới vị trí trên vật thể */
else if(!flag_home && center_count == 2 && count == 1)
{
static bool continue_flag = false;
if(temp == 0)
{
// Đọc tọa độ vật trong thời gian max là 10s
while( ros::ok() && !nova5_ptr_->nova_statusAlarm())
{
if(!nova5_ptr_->nova_checkConnected() || !ros::ok() || nova5_ptr_->nova_statusAlarm() || nova5_ptr_->nova_robotMode() != 7)
{
continue_flag = true;
break;
}
ros::Duration(1).sleep();
ROS_INFO("Read camera");
if(nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam) == 1)
{
// Lấy thời gian của máy tính hiện tại
ros::Time current_time = ros::Time::now();
// Chỉ lấy phần giây và ép kiểu sang int
int seconds = static_cast<int>(current_time.sec);
// Dữ liệu cũ không được cập nhật, chờ cho đến khi nhận được dữ liệu mới nhất
while ((seconds - timeStamp_cam) >= TIME_ERR && ros::ok())
{
ROS_WARN("Data is out of date!");
ROS_WARN("Wait for data update or press ctrl + c to cancel!");
// Lấy thời gian của máy tính hiện tại
ros::Time current_time = ros::Time::now();
// Chỉ lấy phần giây và ép kiểu sang int
int seconds = static_cast<int>(current_time.sec);
nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
// Nếu khoảng thời giữa 2 lần gửi dữ liệu < 1s thì được coi là hợp lệ
if((seconds - timeStamp_cam) < TIME_ERR){
nova5_ptr_->nova_readCoordinates(Point,ORIGIN_COOR,isCheck,timeStamp_cam);
break;
}
if(!nova5_ptr_->nova_checkConnected() || nova5_ptr_->nova_robotMode() == 6)
{
ROS_WARN("Robot is in drag mode");
continue_flag = true;
break;
}
ros::Duration(1).sleep();
}
// Set tọa độ cho điểm trung gian 1
Point_temp1[0] = Point[0];
Point_temp1[1] = Point[1];
Point_temp1[2] = nova5_ptr_->nova_check_z(Point[0],Point[1],ORIGIN_COOR[2], 0);
Point_temp1[3] = Point[3];
Point_temp1[4] = Point[4];
Point_temp1[5] = Point[5];
temp = 1;
break;
}
}
if(continue_flag)
{
continue_flag = false;
continue;
}
// Point_temp1[0] = TEST_COOR[0];
// Point_temp1[1] = TEST_COOR[1];
// Point_temp1[2] = nova5_ptr_->nova_check_z(TEST_COOR[0],TEST_COOR[1],ORIGIN_COOR[2], 0);
// Point_temp1[3] = TEST_COOR[3];
// Point_temp1[4] = TEST_COOR[4];
// Point_temp1[5] = TEST_COOR[5];
temp =1;
}
if(count == 1 && continue_flag == false)
{
if(nova5_ptr_->nova_movePoint(Point_temp1,80,1) == 1) count++;
}
}
/* Bước 4 di chuyển tới vị trí vật thể */
else if(!flag_home && center_count == 2 && count == 2)
{
if(nova5_ptr_->nova_movePoint(Point,20,0)== 1 && count == 2)
{
nova5_ptr_->nova_onTool();
count++;
}
}
/* Bước 5 di chuyển tới vị trí trên vật thể */
else if(!flag_home && center_count == 2 && count == 3)
{
if(nova5_ptr_->nova_movePoint(Point_temp1,40,0)== 1 && count == 3)
{
count++;
}
}
/* Bước 6 di chuyển tới vị trí điểm home */
else if(!flag_home && center_count == 2 &&count == 4)
{
if(nova5_ptr_->nova_movePoint(CENTER_P,100,1)==1 && count == 4)
{
// Nếu vật thể là NG thì set điểm lưu trữ NG_COOR
if(isCheck == 0)
{
NG_COOR[0]+=55;
STORE_COOR[0] = NG_COOR[0];
STORE_COOR[1] = NG_COOR[1];
STORE_COOR[2] = NG_COOR[2];
STORE_COOR[3] = NG_COOR[3];
STORE_COOR[4] = NG_COOR[4];
STORE_COOR[5] = NG_COOR[5];
Point_temp3[0] = NG_COOR[0];
Point_temp3[1] = NG_COOR[1];
Point_temp3[2] = nova5_ptr_->nova_check_z(NG_COOR[0],NG_COOR[1],CENTER_P[2], 0);
Point_temp3[3] = NG_COOR[3];
Point_temp3[4] = NG_COOR[4];
Point_temp3[5] = NG_COOR[5];
speed = 30;
count_ng++;
}
// Nếu vật thể là OK thì set điểm lưu trữ OK_COOR
else if(isCheck == 1)
{
OK_COOR[0]+=55;
STORE_COOR[0] = OK_COOR[0];
STORE_COOR[1] = OK_COOR[1];
STORE_COOR[2] = OK_COOR[2];
STORE_COOR[3] = OK_COOR[3];
STORE_COOR[4] = OK_COOR[4];
STORE_COOR[5] = OK_COOR[5];
Point_temp3[0] = OK_COOR[0];
Point_temp3[1] = OK_COOR[1];
Point_temp3[2] = nova5_ptr_->nova_check_z(OK_COOR[0],OK_COOR[1],CENTER_P[2], 0);
Point_temp3[3] = OK_COOR[3];
Point_temp3[4] = OK_COOR[4];
Point_temp3[5] = OK_COOR[5];
speed = 20;
count_ok++;
}
count++;
}
}
/* Bước 7 di chuyển đến điểm trung gian trên điểm lưu trữ */
else if(!flag_home && center_count == 2 &&count == 5)
{
if(nova5_ptr_->nova_movePoint(Point_temp3,100,1) == 1 && count == 5)
{
count++ ;
}
}
/* Bước 8 di chuyển đến điểm lưu trữ */
else if(!flag_home && center_count == 2 &&count == 6)
{
if(nova5_ptr_->nova_movePoint(STORE_COOR,speed,0) == 1 && count == 6)
{
nova5_ptr_->nova_offTool(0.5);
count++ ;
}
}
/* Bước 9 di chuyển đến điểm trung gian trên điểm lưu trữ */
else if(!flag_home && center_count == 2 &&count == 7)
{
if(nova5_ptr_->nova_movePoint(Point_temp3,speed,0)==1 && count == 7)
{
count++;
}
}
/* Bước 10 reset các biến */
else if(!flag_home && center_count == 2 && count >= 8)
{
// timeStamp_cam_1 = timeStamp_cam;
temp = 0;
count = 0;
center_count = 1;
// if(isCheck > 1) isCheck =0;
if(count_ok > 4 && count_ng > 4) break;
// break;
}
//========================================//
//========================================//
// Hiển thị bước robot đang thực hiện
if((count + center_count) != count_1) ROS_INFO("STEP: %d", count + center_count);
count_1 = count + center_count;
}
else
{
// alarm = nova5_ptr_->nova_statusAlarm();
ROS_WARN("Robot is reconnecting with maximum time 15 minutes...");
for(int i=0; i<180U; ++i)
{
if(!ros::ok()) break;
if(nova5_ptr_->nova_connect() )
{
nova5_ptr_->nova_stopProgram();
// Sau khi enable robot thành công cần bắt đầu chương trình của ccbox
nova5_ptr_->nova_startProgram();
temp = 0;
count = 0;
center_count = 0;
get_point_current = 0;
flag_home = false;
break;
}
ros::Duration(5).sleep();
}
}
loop_rate.sleep();
ros::spinOnce();
}
// nova5_ptr_->nova_stopProgram();
// nova5_ptr_->nova_close();
}

101
Controllers/Packages/nova5_control/example/test_thread.cpp Normal file
View File

@@ -0,0 +1,101 @@
#include "nova5_control/imr_nova_control.h"
#include <iostream>
#include <memory>
#include <ros/ros.h>
#include <std_msgs/UInt16.h>
std::shared_ptr<imr_nova_control> controller = nullptr;
unsigned int ok_count_max = 0;
unsigned int ng_count_max = 1;
bool enable = true;
bool continue_flag = false;
bool go_home_flag = false;
bool power_on_flag = false;
void callBack(const std_msgs::UInt16::ConstPtr& msg)
{
if(controller != nullptr)
{
switch (msg->data)
{
case 1:
controller->startHomeThread();
// continue_flag = true;
// go_home_flag = false;
// power_on_flag = false;
break;
case 2:
// continue_flag = false;
// go_home_flag = true;
// power_on_flag = false;
break;
case 3:
// continue_flag = false;
// go_home_flag = false;
// power_on_flag = true;
break;
default:
// continue_flag = false;
// go_home_flag = false;
// power_on_flag = false;
break;
}
}
}
int main(int argc, char** argv) {
// Initialize the ROS node
ros::init(argc, argv, "test_imr_nova_control");
ros::NodeHandle nh = ros::NodeHandle("~");
ros::Subscriber sub = nh.subscribe("mode", 1, callBack);
ros::Publisher pub = nh.advertise<std_msgs::UInt16>("status", 1);
try {
// Initialize ROS time
// ros::Time::init();
// Create an instance of imr_nova_control
controller = std::make_shared<imr_nova_control>();
controller->ok_count_max_ = &ok_count_max;
controller->ng_count_max_ = &ng_count_max;
controller->enable_ = &enable;
controller->continue_flag_ = &continue_flag;
controller->go_home_flag_ = &go_home_flag;
controller->power_on_flag_ = &power_on_flag;
#if 0
controller->startHomeThread();
#endif
#if 0
controller->startModeThread();
#endif
std::cout << "imr_nova_control instance created successfully." << std::endl;
// Run ROS spin to process callbacks (if applicable)
// ros::AsyncSpinner spinner(2); // Adjust the number of threads as needed
// spinner.start();
ros::Rate rate(5);
while (ros::ok())
{
std_msgs::UInt16 msg;
msg.data = controller->statusCode_;
pub.publish(msg);
ros::spinOnce();
rate.sleep();
}
} catch (const std::exception& e) {
std::cerr << "An error occurred: " << e.what() << std::endl;
}
ros::spin();
return 0;
}