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

76
Devices/Libraries/Ros/md_controller/src/diagnostic.cpp Executable file
View File

@@ -0,0 +1,76 @@
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include "md_controller/diagnostic.h"
/**
* g_diagnostic_handler: singleton, implements the diagnostic for sick_line_guidance
*/
MD::Diagnostic::DiagnosticImpl MD::Diagnostic::g_diagnostic_handler;
/*
* Constructor.
*/
MD::Diagnostic::DiagnosticImpl::DiagnosticImpl() : m_diagnostic_initialized(false), m_diagnostic_component("")
{
}
/*
* Initialization.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
void MD::Diagnostic::DiagnosticImpl::init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component)
{
m_diagnostic_publisher = nh.advertise<diagnostic_msgs::DiagnosticArray>(publish_topic, 1);
m_diagnostic_component = component;
m_diagnostic_initialized = true;
}
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
void MD::Diagnostic::DiagnosticImpl::update(DIAGNOSTIC_STATUS status, const std::string &message)
{
if (m_diagnostic_initialized)
{
static std::map<DIAGNOSTIC_STATUS, std::string> status_description = {
{DIAGNOSTIC_STATUS::OK, "OK"},
{DIAGNOSTIC_STATUS::EXIT, "EXIT"},
{DIAGNOSTIC_STATUS::ERROR_STATUS, "ERROR_STATUS"},
{DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "CONFIGURATION_ERROR"},
{DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "INITIALIZATION_ERROR"},
{DIAGNOSTIC_STATUS::INTERNAL_ERROR, "INTERNAL_ERROR"}
};
// create DiagnosticStatus
diagnostic_msgs::DiagnosticStatus msg;
msg.level = (status == DIAGNOSTIC_STATUS::OK) ? (diagnostic_msgs::DiagnosticStatus::OK) : (diagnostic_msgs::DiagnosticStatus::ERROR); // Level of operation
msg.name = m_diagnostic_component; // description of the test/component reporting
msg.hardware_id = ""; // hardware unique string (tbd)
msg.values.clear(); // array of values associated with the status
// description of the status
msg.message = status_description[status];
if(msg.message.empty())
{
msg.message = "ERROR";
}
if (!message.empty())
{
msg.message = msg.message + ": " + message;
}
// publish DiagnosticStatus in DiagnosticArray
diagnostic_msgs::DiagnosticArray msg_array;
msg_array.header.stamp = ros::Time::now();
msg_array.header.frame_id = "";
msg_array.status.clear();
msg_array.status.push_back(msg);
m_diagnostic_publisher.publish(msg_array);
}
}

666
Devices/Libraries/Ros/md_controller/src/md_controller.cpp Executable file
View File

@@ -0,0 +1,666 @@
#include "md_controller/md_controller.h"
MD::MdController::~MdController(){}
/*
* Object
* @param devfile Path name of dev
* @param baud Baudrate
* @param parity Parity
* @param data_bit Data bit
* @param stop_bit Stop bit
*/
MD::MdController::MdController(ros::NodeHandlePtr nh, const char *devfile, unsigned int baud, parity_t parity, data_bits_t data_bit,stop_bits_t stop_bit)
: nh_(nh), rs485(devfile,baud, parity, data_bit, stop_bit)
{
if(this->_connected == true)
{
if(!this->check_baudrate(baud))
{
std::stringstream msg;
msg << "DRIVER not support baudrate" << baud << "bps";
MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, msg.str());
ROS_ERROR_STREAM("md_controller_node: " << msg.str());
this->close_port();
exit(-1);
}
else
{
std::stringstream msg;
msg << " Connected "<<devfile<<" with "<<baud<<" bps";
this->err = false;
this->err_no = 0;
ROS_INFO_STREAM("md_controller_node: " << msg.str());
ros::Duration(0.7).sleep(); // sleep for 0.5 second
MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::OK, msg.str());
}
}
else
{
ros::Duration(2).sleep();
std::stringstream err;
err << error_msg;
ROS_ERROR_STREAM(error_msg);
MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, error_msg);
}
}
// /**
// * Get parameter from yaml file
// */
// void MD::MdController::loadParam(void)
// {
// int node_id[this->drivers.size()];
// int count = 0;
// for(XmlRpc::XmlRpcValue::iterator _iter = this->drivers.begin(); _iter != this->drivers.end(); ++_iter)
// {
// std::string driver_name = readMember<std::string>(_iter->second, "name", _iter->first);
// if(_iter->second.hasMember("id"))
// {
// node_id[count] = _iter->second["id"];
// ROS_INFO("md_controller: initializing driver_name \"%s\", id \"0x0%d\"...", driver_name.c_str(),node_id[count]);
// count++;
// } else
// {
// ROS_ERROR_STREAM("Node '" << _iter->first << "' has no id");
// MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " Node name has no id");
// }
// }
// this->set_slave_id(node_id,count);
// }
/*
* Check baudrate has supported for device
* @param baud Package Array size byte
* @param return true/false
*/
bool MD::MdController::check_baudrate(unsigned int baud)
{
bool status = false;
unsigned int data_defaut[] = {9600, 19200, 38400, 57600, 115200};
for(uint8_t i = 0; i < (uint8_t)sizeof(data_defaut)/sizeof(unsigned int); i++)
{
if(baud == data_defaut[i])
{
status = true;
break;
}
}
return status;
}
/*
* Checksum
* @param length Package Array size byte
* @param to_send Package Array
* @param return byte
*/
uint8_t MD::MdController::GetcheckSum(uint length, uint8_t *to_send)
{
uint8_t byTmp = 0;
for(short i = 0; i < length; i++ ) byTmp += *(to_send + i);
return (~byTmp +1);
} //GetcheckSum
/**
* Set connection is bad
*/
void MD::MdController::set_bad_con(void)
{
this->err = true;
ROS_ERROR("BAD CONNECTION");
}
/*
* Modbus Request Builder
* @param to_send Message Buffer input
* @param hd Header
* @param add ID Number
* @param pid Parameter ID
* @param num Data number
*/
void MD::MdController::buidRequest(uint8_t *to_send,header_t hd ,
uint8_t address, param_id_t pid, dt_num_t num) const
{
to_send[0] = hd.RMID;
to_send[1] = hd.TMID;
to_send[2] = address;
to_send[3] = pid.PID;
to_send[4] = num.NUM;
} // buidRequest
/*
* Write Request Builder and Sender
* @param address Reference Address
* @param lenght Amount of data to be Written
* @param value Data to Be Written
*/
int MD::MdController::Write(header_t hd ,uint8_t address, param_id_t pid,
dt_num_t num, uint8_t *value)
{
int status = 0;
uint length = num.NUM + 5;
uint8_t to_send[length+1];
this->buidRequest(to_send,hd,address,pid,num);
for(uint8_t i = 0; i < num.NUM; i++) to_send[length -num.NUM +i] = value[i];
to_send[length] = this->GetcheckSum(length,to_send);
struct stat sb;
if(stat(this->ctx._port, &sb) < 0) this->reconnect();
if(this->_connected)
{
status = this->sendMsgs(to_send,length+1);
}
return status;
}
/*
* Read Request Builder and Sender
* @param address Reference Address
* @param lenght Amount of Data to Read
* @param return EXIT_CODES Type
*/
int MD::MdController::Read(uint8_t *read_buf, ssize_t &num)
{
ssize_t k = this->receiveMsgs(read_buf);
num = k;
if(k < 0)
{
this->set_bad_con();
return EXIT_CONFIG_ERROR;
}
else{
return EXIT_OK;
}
}
/*
* Default setting
* @param address ID
* Data : 0x55(CHECK)
* 183, 172, ID, 3, 1, 0x55, CHK
*/
void MD::MdController::pid_defaut_set(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 3;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0x55u; //Data : 0x55(CHECK)
this->Write(hd,address,pid,num,value);
}
/*
* Stop naturally
* @param address ID
* Stop motor naturally, data don’t care(x).
* 183, 172, ID, 5, 1, x, CHK
*/
void MD::MdController::pid_tq_off(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 5;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0X01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* Erectric brake
* @param address ID
* Stop motor urgently(electric braking mode)
* 183, 172, ID, 6, 1, x, CHK
*/
void MD::MdController::pid_break(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 6;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0x01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* stop
* @param address ID
* Stop motor urgently(electric braking mode)
* 183, 172, ID, 6, 1, x, CHK
*/
void MD::MdController::pid_stop(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 5;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0x01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* Pid command
* @param address ID
* @param cmd Contents on CMD number (forllow page 6-CommunicationProtocolOnRS485_V44)
* 183, 172, ID, 10, 1, CMD, CHK
*/
void MD::MdController::pid_command(uint8_t address, PID_COMMAND cmd)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 10;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = cmd;
this->Write(hd,address,pid,num,value);
}
/*
* Reset alarm
* @param address ID
* Data don’t care(x)
* 183, 172, ID, 12, 1, x, CHK
*/
void MD::MdController::pid_alarm_reset(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 12;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0X01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* Reset position, Motor position to zero
* @param address ID
* Data don’t care(x)
* 183, 172, ID, 13, 1, x, CHK
*/
void MD::MdController::pid_posi_reset(uint8_t address)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 13;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = 0X01u; //data don’t care(x).
this->Write(hd,address,pid,num,value);
}
/*
* Request broadcasting of PID_MAIN_DATA
* @param address ID
* @param data DATA 1 : PID 193 broadcasting on OR 0 : broasdcasting off
* 183, 172, ID, 14, 1, DATA, CHK
*/
void MD::MdController::pid_main_bc_state(uint8_t address, _BIT data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 14;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = data;
this->Write(hd,address,pid,num,value);
}
/*
* Request BC on/off of PID_MONITOR
* @param address ID
* @param data DATA 1 : PID 196 broadcasting on OR 0 : broasdcasting off
* 183, 172, ID, 15, 1, DATA, CHK
*/
void MD::MdController::pid_monitor_bc_state(uint8_t address, _BIT data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 15;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
value[num.NUM - 1] = data;
this->Write(hd,address,pid,num,value);
}
/*
* Save preset position
* @param address ID
* @param data DATA Preset number(address, 1~20)
* Set current position to the preset address
* 183, 172, ID, 31, 1, DATA, CHK
*/
void MD::MdController::pid_preset_save(uint8_t address, uint8_t data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 31;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
if(data > 0 && data <=20)
{
value[num.NUM - 1] = data;
this->Write(hd,address,pid,num,value);
}
else
{
ROS_ERROR("Command 'PID_PRESET_SAVE' element 'data' number must in ranges (1~20). Prefer page 10 CommunicationProtocolOnRS485_V44");
}
}
/*
* Go to the recalled preset position
* @param address ID
* @param data DATA Preset number(address, 1~20)
* Recall the saved preset data and move to thatposition.(position control)
* 183, 172, ID, 32, 1, DATA, CHK
*/
void MD::MdController::pid_preset_recall(uint8_t address, uint8_t data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 32;
dt_num_t num; num.NUM = 1;
uint8_t value[num.NUM];
if(data > 0 && data <=20)
{
value[num.NUM - 1] = data;
this->Write(hd,address,pid,num,value);
}
else
{
ROS_ERROR("Command 'PID_PRESET_RECALL' element 'data' number must in ranges (1~20). Prefer page 10 CommunicationProtocolOnRS485_V44");
}
}
/*
* Velocity command
* @param address ID
* @param speed Speed(rpm) = (D1 | D2<<8)
* Speed>0, CCW direction
* Speed<0, CW direction
* 183, 172, ID, 130, 2, D1, D2, CHK
*/
void MD::MdController::pid_vel_cmd(uint8_t address, int16_t speed)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 130;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
value[num.NUM - 2] = speed & 0x00FFu;
value[num.NUM - 1] = speed >> 8u;
this->Write(hd,address,pid,num,value);
}
/*
* Velocity command used more than 25,000rpm
* @param address ID
* @param high_speed Speed(rpm) = (D1 | D2<<8) x 10
* Speed>0, CCW direction
* Speed<0, CW direction
* 183, 172, ID, 131, 2, D1, D2, CHK
*/
void MD::MdController::pid_vel_cmd2(uint8_t address, int16_t high_speed)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 131;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
value[num.NUM - 2] = high_speed*10 & 0x00FFu;
value[num.NUM - 1] = high_speed*10 >> 8u;
this->Write(hd,address,pid,num,value);
}
/*
* Velocity command used more than 25,000rpm
* @param address ID
* @param data D1, D2: Open-loop velocity
* Range : -1023~1023
* 183, 172, ID, 134, 2, D1, D2, CHK
*/
void MD::MdController::pid_open_vel_cmd(uint8_t address, int16_t data)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 134;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
if(data >= -1023 && data <= 1023)
{
value[num.NUM - 2] = data & 0x00FFu;
value[num.NUM - 1] = data >> 8u;
this->Write(hd,address,pid,num,value);
}
else
{
ROS_ERROR("Command 'PID_PRESET_RECALL' element 'data' number must in ranges (-1023 ~ 1023). Prefer page 10 CommunicationProtocolOnRS485_V44");
}
}
/*
* ID setting
* @param id_setting ID : 1~253 : setting ID
* Write command(0xaa)
* 183, 172, 254, 133, 2, 0xaa, ID, CHK
*/
void MD::MdController::pid_id_setting(uint8_t id_setting)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 133;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
if(id_setting > 0 && id_setting <=253)
{
value[num.NUM - 2] = 0xAAu;
value[num.NUM - 1] = id_setting;
this->Write(hd,254,pid,num,value);
}
else
{
ROS_ERROR("Command 'PID_ID' element 'id_setting' number must in ranges (1 ~ 253). Prefer page 12 CommunicationProtocolOnRS485_V44");
}
}
/*
* Set the baudrate, BAUD
* @param address ID
* @param baudrate_setting Baudrate setting(RS485)
* Write command(0xaa)
* 183, 172, ID, 135, 2, 0xaa, BAUD, CHK
*/
void MD::MdController::pid_baudrate_setting(uint8_t address, _BAUD_OF_MD200 baudrate_setting)
{
header_t hd; hd.TMID = 184;
param_id_t pid; pid.PID = 133;
dt_num_t num; num.NUM = 2;
uint8_t value[num.NUM];
value[num.NUM - 2] = 0xAAu;
value[num.NUM - 1] = (uint8_t)baudrate_setting;
this->Write(hd,address,pid,num,value);
}
// /*
// * Set slave id
// * @param ids Pointer save array slave id
// * @param lenght Lenght of ids array (sizeof(ids)/sizeof(uint8_t))
// */
// void MD::MdController::set_slave_id(int *ids, uint8_t lenght)
// {
// uint8_t n = lenght;
// for(uint8_t i = 0; i < n; i++)
// {
// for(int j = 0; j < n; j++)
// {
// if(ids[j] == ids[i] && j != i)
// {
// ids[j] = ids[j+1];
// n--;
// }
// }
// }
// if(n != lenght) ROS_WARN("There are %d ID input to be same!!", lenght - n);
// this->slave_id.size = n;
// for(uint8_t k = 0; k < n; k++) this->slave_id.adr[k] = ids[k];
// }
// /*
// * Set slave id
// * @param speed Speed list input
// */
// void MD::MdController::publish_speed(int16_t *speed)
// {
// if(this->slave_id.size > 0)
// {
// for(uint8_t i = 0; i < this->slave_id.size; i++)
// {
// if(abs(speed[i]) > 0)
// this->pid_vel_cmd(this->slave_id.adr[i],speed[i]);
// else
// this->pid_break(this->slave_id.adr[i]);
// }
// }else
// {
// ros::Duration(3).sleep(); // sleep for 3 second
// ROS_ERROR("There are not slave ID of devices MD Motor");
// }
// }
// /*
// * PID_MAIN_DATA broadcasting ON
// * @param CMD_MAIN_BC_ON 5
// */
// void MD::MdController::main_BC_ON()
// {
// if(this->slave_id.size > 0)
// {
// for(uint8_t i = 0; i < this->slave_id.size; i++)
// {
// this->pid_command(this->slave_id.adr[i],CMD_MAIN_BC_ON);
// usleep(30000);
// }
// this->theadEnable = true;
// if(this->ThreadOne.joinable() == false)
// {
// this->ThreadOne = boost::thread(this->cmdMainBCdoStuff,this,&this->rate);
// }
// }else
// {
// ros::Duration(3).sleep(); // sleep for 3 second
// ROS_ERROR("There are not slave ID of devices MD Motor");
// }
// }
// /*
// * PID_MAIN_DATA broadcasting OFF
// * @param CMD_MAIN_BC_OFF 6
// */
// void MD::MdController::main_BC_OFF()
// {
// if(this->slave_id.size > 0)
// {
// for(uint8_t i = 0; i < this->slave_id.size; i++)
// {
// this->pid_command(this->slave_id.adr[i],CMD_MAIN_BC_OFF);
// usleep(30000);
// }
// this->theadEnable = false;
// if(this->ThreadOne.joinable() == true)
// this->ThreadOne.join();
// }else
// {
// ros::Duration(3).sleep(); // sleep for 3 second
// ROS_ERROR("There are not slave ID of devices MD Motor");
// }
// }
// /*
// * Read data
// * @param payload This
// * @param publish_rate Loop rate
// */
// void MD::MdController::cmdMainBCdoStuff(void *payload, uint8_t *publish_rate)
// {
// MD::MdController *as = (MD::MdController *)payload;
// ros::Rate loop_rate(*publish_rate);
// ROS_INFO("TheadOne to read msg devices motor is ready !!");
// while(ros::ok() && as->theadEnable)
// {
// ssize_t num;
// uint8_t msg[256];
// as->Read(msg,num);
// as->cs.w_index += num;
// if(as->cs.w_index >= MAX_BUFFER_LENGHT) {
// uint8_t l_part = MAX_BUFFER_LENGHT - (as->cs.w_index - num);
// for(uint8_t i = 0; i < l_part; i++) as->cs._buffer[as->cs.w_index - num + i] = msg[i];
// as->cs.w_index = num - l_part;
// for(uint8_t i = l_part; i < num; i++) as->cs._buffer[as->cs.w_index - num + l_part + i] = msg[i];
// as->cs.c_flag++;
// }
// else {
// for(uint8_t i = 0; i < num; i++) as->cs._buffer[as->cs.w_index - num + i] = msg[i];
// }
// if(as->cs.w_index - as->cs.r_index > PACKAGE_MSG_MAIN_DATA || as->cs.c_flag > 0)
// {
// if(as->cs._buffer[as->cs.r_index] == 172 && as->cs._buffer[as->cs.r_index + 1] == 183)
// {
// int num_data = as->cs._buffer[as->cs.r_index + 4];
// if(as->cs.w_index - as->cs.r_index >= 6 + num_data)
// {
// as->cs.r_index += 6 + num_data;
// if(as->cs.r_index >= MAX_BUFFER_LENGHT)
// {
// as->cs.c_flag--;
// uint8_t l_part = MAX_BUFFER_LENGHT - (as->cs.r_index - PACKAGE_MSG_MAIN_DATA);
// for(uint8_t i = 0; i < l_part; i++)
// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + i];
// as->cs.r_index = PACKAGE_MSG_MAIN_DATA - l_part;
// for(uint8_t i = l_part; i < PACKAGE_MSG_MAIN_DATA; i++)
// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + l_part + i ];
// }
// else
// {
// for(uint8_t i = 0; i < PACKAGE_MSG_MAIN_DATA; i++)
// as->cs.result[i] = as->cs._buffer[as->cs.r_index - PACKAGE_MSG_MAIN_DATA + i];
// }
// std::cout << "num_data " << std::dec << num_data << " ";
// // std::cout << "w_index " << std::dec << (int)(as->cs.w_index) << " r_index = " << std::dec << (int)(as->cs.r_index) << " ";
// // std::cout << " c_flags = " << std::dec << (int)(as->cs.c_flag);
// //std::cout << " result = ";
// // for(int i = 0; i < 22; i++)
// // {
// // std::cout << std::hex << (int)as->cs.result[i] << " ";
// // }
// std::cout << std:: endl;
// }
// }
// else
// {
// std::cout << " buffer = ";
// for(int i = as->cs.r_index; i < as->cs.r_index + 22; i++)
// {
// std::cout << std::hex << (int)as->cs._buffer[i] << " ";
// }
// std::cout << std:: endl;
// as->cs.r_index ++;
// }
// if(as->cs.c_flag > 1)
// {
// ROS_WARN("ERROR DATA");
// exit(-1);
// }
// }
// std::cout << "w_index = " << std::dec << (int)(as->cs.w_index) << " r_index = " << std::dec << (int)(as->cs.r_index) << " ";
// std::cout << " c_flags = " << std::dec << (int)(as->cs.c_flag);
// // std::cout << " buffer = ";
// // for(int i = 0; i < 300; i++)
// // {
// // std::cout << std::hex << (int)as->cs._buffer[i] << " ";
// // }
// std::cout << std:: endl;
// loop_rate.sleep();
// ros::spinOnce();
// }
// }

146
Devices/Libraries/Ros/md_controller/src/md_controller_node.cpp Executable file
View File

@@ -0,0 +1,146 @@
#include <ros/ros.h>
#include <XmlRpcValue.h>
#include <boost/thread/thread.hpp>
#include "md_controller/md_controller.h"
#include "md_controller/diagnostic.h"
#include "md_controller/md_controller_subscriber.h"
#include "libserial/udevadm_info.h"
#include <XmlRpcException.h>
/* define struct */
typedef struct device{
std::string _port;
int _baud;
}device_t;
bool findDevice(ros::NodeHandlePtr nh, std::string node_name, device_t *dv) {
ROS_INFO("loadParam() - node_name: %s", node_name.c_str());
if(!nh->param(node_name + "/baudrate", dv->_baud, dv->_baud))
{
if(!nh->getParam("baudrate", dv->_baud))
return false;
}
ROS_INFO("product");
std::string product; //port name
if(!nh->param(node_name + "/product", product, product)) {}
else if(!nh->getParam("product", product)) {}
else
{
udevadmInfo *ludev = new udevadmInfo(product.c_str());
if(ludev->init() == EXIT_FAILURE) {}
if(ludev->scanDevices())
{
strcpy(dv->_port.data(), ludev->port);
return true;
}
}
ROS_INFO("port_name");
if(!nh->param(node_name + "/port_name", dv->_port, dv->_port))
{
if(!nh->getParam("port_name", dv->_port))
return false;
}
ROS_WARN("Can not scan device from /product. So try to use /port_name is %s", dv->_port.c_str());
return true;
}
/**
* @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
*/
template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
{
try
{
if(value.hasMember(member))
return value[member];
return defaultvalue;
}
catch(const XmlRpc::XmlRpcException& e)
{
ROS_WARN_STREAM( "'"<<member << "' "<< e.getMessage() << '\n');
return defaultvalue;
}
}
/**********************************************************************
* MAIN
***********************************************************************/
int main(int argc,char **argv)
{
/* create ros ndoe */
ros::init(argc, argv, "MD");
ros::NodeHandlePtr nh = boost::make_shared<ros::NodeHandle>();
std::string node_name = ros::this_node::getName();
ROS_INFO("%s.cpp-node_name: %s", node_name.c_str(), node_name.c_str());
/* connect with the server */
std::string diagnostic_topic = "MD_diagnostic";
MD::Diagnostic::init(*nh, diagnostic_topic, "MD md_controller");
XmlRpc::XmlRpcValue drivers;
if(!nh->getParam("drivers", drivers) || drivers.size() < 1)
{
ROS_ERROR("%s: no driver found in yaml-file, please check configuration. Aborting...", node_name.c_str());
MD::Diagnostic::update(MD::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " No driver found in yaml-file");
return 2;
}
std::vector<boost::shared_ptr<MD::Subscriber>> p_md_controller_subscriber;
for(XmlRpc::XmlRpcValue::iterator object_iter = drivers.begin(); object_iter != drivers.end(); ++object_iter)
{
std::string topic_name = readMember<std::string>(object_iter->second, "toppic", "");
int subscribe_queue_size = readMember<int>(object_iter->second, "subscribe_queue_size", 1);
ROS_INFO("%s: initializing toppic \"%s\" queue_size \"%d\"...", node_name.c_str(), topic_name.c_str(), subscribe_queue_size);
p_md_controller_subscriber.push_back(boost::make_shared<MD::Subscriber>(*nh, object_iter));
}
device_t dv;
if(findDevice(nh, node_name, &dv))
{
ROS_INFO("%s: found device successfull", node_name.c_str());
}
else
{
ROS_ERROR("%s: Error when find device", node_name.c_str());
return 2;
}
uint8_t rate = 20; // Hz
ros::Rate loop_rate(rate);
while (ros::ok())
{
ROS_INFO("%s: Conecting port name %s with baudrate %d ...", node_name.c_str(), dv._port.c_str(), dv._baud);
boost::shared_ptr<MD::MdController> md_controller = boost::make_shared<MD::MdController>(nh, dv._port.c_str(), dv._baud, PARITY_NONE, DATABIT_8, STOPBIT_1);
while(ros::ok() && md_controller->_connected)
{
for(auto &p_sub : p_md_controller_subscriber)
{
if(p_sub->isWheelTriggered())
{
double rotate_speed_wheel;
{
boost::lock_guard<boost::mutex> publish_lockguard(p_sub->m_measurement_mutex);
rotate_speed_wheel = p_sub->speed;
}
if(abs(rotate_speed_wheel) > 0)
md_controller->pid_vel_cmd(p_sub->slave_id, rotate_speed_wheel);
else
md_controller->pid_break(p_sub->slave_id);
p_sub->scheduleWheelController(false);
}
}
loop_rate.sleep();
ros::spinOnce();
}
ros::spinOnce();
md_controller->close_port();
}
ros::spin();
return 0;
}

102
Devices/Libraries/Ros/md_controller/src/md_controller_subscriber.cpp Executable file
View File

@@ -0,0 +1,102 @@
#include "md_controller/md_controller_subscriber.h"
#include <XmlRpcException.h>
/**
* @brief shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
*/
template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
{
try
{
if(value.hasMember(member))
return value[member];
return defaultvalue;
}
catch(const XmlRpc::XmlRpcException& e)
{
return defaultvalue;
}
}
MD::Subscriber::Subscriber(ros::NodeHandle & nh, const std::string & ros_topic, int subscribe_queue_size, double schedule_delay, double schedule_delay_lastest)
: nh_(nh), slave_id(0),
topic_name_(ros_topic),
m_subscribe_queue_size_(subscribe_queue_size),
schedule_delay_(ros::Duration(schedule_delay)),
schedule_lastest_delay_(ros::Duration(schedule_delay_lastest)),
publish_time_(ros::Time(0)),
publish_lastest_time_(ros::Time(0))
{
control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this);
}
MD::Subscriber::Subscriber(ros::NodeHandle & nh, XmlRpc::XmlRpcValue::iterator object)
: nh_(nh), slave_id(0),
publish_time_(ros::Time(0)),
publish_lastest_time_(ros::Time(0))
{
topic_name_ = readMember<std::string>(object->second, "toppic", "");
m_subscribe_queue_size_ = readMember<int>(object->second, "subscribe_queue_size", 1);
slave_id = (uint8_t)readMember<int>(object->second, "id", 1);
ratio = (double)readMember<int>(object->second, "ratio", 0.0);
ratio = readMember<double>(object->second, "ratio", ratio);
double schedule_delay = readMember<double>(object->second, "schedule_delay", 0.05);
double schedule_delay_lastest = readMember<double>(object->second, "schedule_delay_lastest", 0.5);
this->schedule_delay_ = ros::Duration(schedule_delay);
this->schedule_lastest_delay_ = ros::Duration(schedule_delay_lastest);
control_wheel_sub_ = nh_.subscribe(topic_name_, m_subscribe_queue_size_, &MD::Subscriber::wheelCallback, this);
}
MD::Subscriber::~Subscriber() = default;
void MD::Subscriber::wheelCallback(const std_msgs::Float32::Ptr & msg)
{
boost::lock_guard<boost::mutex> schedule_lockguard(this->m_measurement_mutex);
// check that we don't have multiple publishers on the command topic
if (control_wheel_sub_.getNumPublishers() > 1)
{
ROS_ERROR_STREAM_THROTTLE(1.0, "Detected " << control_wheel_sub_.getNumPublishers()
<< " publishers " << topic_name_ << ". Only 1 publisher is allowed. Going to brake.");
speed = 0;
return;
}
if (!std::isfinite(msg->data))
{
ROS_WARN_THROTTLE(1.0, "Received NaN in %s command. Ignoring.", topic_name_.c_str());
speed = 0;
return;
}
speed = (int16_t)(msg->data * ratio * 60 / 2 / M_PI);
this->scheduleWheelController(true);
}
bool MD::Subscriber::isWheelTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
return !publish_time_.isZero() && ros::Time::now() > publish_time_;
}
bool MD::Subscriber::isWheelLastestTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
}
void MD::Subscriber::scheduleWheelController(bool schedule)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
if(schedule && publish_time_.isZero())
{
publish_time_ = ros::Time::now() + schedule_delay_;
publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
}
if(!schedule && !publish_time_.isZero())
{
publish_time_ = ros::Time(0);
publish_lastest_time_ = ros::Time(0);
}
}