int main(int argc, char** argv)
{
ros::init(argc, argv, "elektron_base_node");
ros::NodeHandle n;
ros::NodeHandle nh("~");
bool publish_odom_tf;
bool dump;
double ls, rs;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry> ("odom", 1);
tf::TransformBroadcaster odom_broadcaster;
ros::Subscriber twist_sub = n.subscribe("cmd_vel", 1, &twistCallback);
ros::Rate loop_rate(200);
std::string dev;
if (!nh.getParam("device", dev))
{
dev = "/dev/ttyACM0";
}
if (!nh.getParam("publish_odom_tf", publish_odom_tf))
{
publish_odom_tf = false;
}
if (!nh.getParam("dump", dump))
{
dump = false;
}
if (!nh.getParam("lin_scale", ls))
{
ls = 1.0;
}
if (!nh.getParam("rot_scale", rs))
{
rs = 1.0;
}
nav_msgs::Odometry odom;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
// NFv2 Configuration
NFv2_Config(&NFComBuf, NF_TerminalAddress);
CommPort = new SerialComm(dev);
if (! (CommPort->isConnected()) )
{
ROS_INFO("connection failed");
ROS_ERROR("Connection to device %s failed", dev.c_str());
return 0;
}
else
{
ROS_INFO("connection ok");
pthread_t listenerThread;
if(pthread_create(&listenerThread, NULL, &listener, NULL))
ROS_ERROR("Listener thread error");
else
ROS_INFO("Listener thread started");
ros::Timer timer1 = n.createTimer(ros::Duration(0.1), readDeviceVitalsTimerCallback);
while (ros::ok())
{
double x, y, th, xvel, thvel;
ros::Time current_time = ros::Time::now();
// If communication with Elektron requested
if(commandCnt > 0)
{
txCnt = NF_MakeCommandFrame(&NFComBuf, txBuf, (const uint8_t*)commandArray, commandCnt, NF_RobotAddress);
// Clear communication request
commandCnt = 0;
// Send command frame to Elektron
CommPort->write(txBuf, txCnt);
timeoutCount = 0;
}/*else
{
if(timeoutCount < TIMEOUT)
{
txCnt = NF_MakeCommandFrame(&NFComBuf, txBuf, (const uint8_t*)commandArray, commandCnt, NF_RobotAddress);
CommPort->write(txBuf, txCnt);
commandCnt = 0;
timeoutCount++;
}
else
{
NFComBuf.SetDrivesSpeed.data[0] = 0;
NFComBuf.SetDrivesSpeed.data[1] = 0;
commandArray[commandCnt++] = NF_COMMAND_SetDrivesSpeed;
NFComBuf.SetDrivesMode.data[0] = NF_DrivesMode_SPEED;
NFComBuf.SetDrivesMode.data[1] = NF_DrivesMode_SPEED;
commandArray[commandCnt++] = NF_COMMAND_SetDrivesMode;
txCnt = NF_MakeCommandFrame(&NFComBuf, txBuf, (const uint8_t*)commandArray, commandCnt, NF_RobotAddress);
CommPort->write(txBuf, txCnt);
commandCnt = 0;
}
}
*/
x = 0;
y = 0;
th = 0;
xvel = 0;
thvel = 0;
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
if (publish_odom_tf)
{
//first, we'll publish the transform over tf
odom_trans.header.stamp = current_time;
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
}
//next, we'll publish the odometry message over ROS
odom.header.stamp = current_time;
//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.pose.covariance[0] = 0.00001;
odom.pose.covariance[7] = 0.00001;
odom.pose.covariance[14] = 10.0;
odom.pose.covariance[21] = 1.0;
odom.pose.covariance[28] = 1.0;
odom.pose.covariance[35] = thvel + 0.001;
//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = xvel;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.angular.z = thvel;
//publish the message
//odom_pub.publish(odom);
ros::spinOnce();
loop_rate.sleep();
}
}
return 0;
}
//do communication cycle
uint64_t HI_moxa::HI_read_write_hardware(void)
{
static int64_t receive_attempts = 0;
// UNUSED: static int64_t receive_timeouts = 0;
static int error_msg_power_stage = 0;
static int error_msg_hardware_panic = 0;
static int error_msg_overcurrent = 0;
static int last_synchro_state[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
static int comm_timeouts[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
static int synchro_switch_filter[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
const int synchro_switch_filter_th = 2;
bool robot_synchronized = true;
bool power_fault;
bool all_hardware_read = true;
uint64_t ret = 0;
uint8_t drive_number;
static int status_disp_cnt = 0;
static std::stringstream temp_message;
// If Hardware Panic, send PARAM_DRIVER_MODE_ERROR to motor drivers
if (hardware_panic) {
for (drive_number = 0; drive_number <= last_drive_number; drive_number++) {
// only set error parameter, do not wait for answer
servo_data[drive_number].commandCnt = 0;
NFComBuf.SetDrivesMode.data[drive_number] = NF_DrivesMode_ERROR;
servo_data[drive_number].commandArray[servo_data[drive_number].commandCnt++] = NF_COMMAND_SetDrivesMode;
txCnt =
NF_MakeCommandFrame(&NFComBuf, txBuf + 5, (const uint8_t*) servo_data[drive_number].commandArray, servo_data[drive_number].commandCnt, drives_addresses[drive_number]);
// Clear communication request
servo_data[drive_number].commandCnt = 0;
// Send command frame
SerialPort[drive_number]->write(txBuf, txCnt + 5);
}
if (error_msg_hardware_panic == 0) {
std::cout << "[error] hardware panic" << std::endl;
error_msg_hardware_panic++;
}
std::cout << temp_message.str() << std::endl;
struct timespec delay;
delay.tv_nsec = 2000000;
delay.tv_sec = 0;
nanosleep(&delay, NULL);
return ret;
} else {
// Make command frames and send them to drives
for (drive_number = 0; drive_number <= last_drive_number; drive_number++) {
// Set communication requests
servo_data[drive_number].commandArray[servo_data[drive_number].commandCnt++] = NF_COMMAND_ReadDrivesPosition;
servo_data[drive_number].commandArray[servo_data[drive_number].commandCnt++] = NF_COMMAND_ReadDrivesCurrent;
servo_data[drive_number].commandArray[servo_data[drive_number].commandCnt++] = NF_COMMAND_ReadDrivesStatus;
// Make command frame
servo_data[drive_number].txCnt =
NF_MakeCommandFrame(&NFComBuf, servo_data[drive_number].txBuf + howMuchItSucks, (const uint8_t*) servo_data[drive_number].commandArray, servo_data[drive_number].commandCnt, drives_addresses[drive_number]);
// Clear communication requests
servo_data[drive_number].commandCnt = 0;
}
for (drive_number = 0; drive_number <= last_drive_number; drive_number++) {
// Send command frame
SerialPort[drive_number]->write(servo_data[drive_number].txBuf, servo_data[drive_number].txCnt + howMuchItSucks);
}
}
receive_attempts++;
struct timespec delay;
delay.tv_nsec = 1500000;
delay.tv_sec = 0;
nanosleep(&delay, NULL);
// Tu kiedys byl SELECT
all_hardware_read = true;
// Read data from all drives
for (drive_number = 0; drive_number <= last_drive_number; drive_number++) {
rxCnt = 0;
while (1) {
if (SerialPort[drive_number]->read(&(rxBuf[rxCnt]), 1) > 0 && (rxCnt < 255)) {
if (NF_Interpreter(&NFComBuf, rxBuf, &rxCnt, rxCommandArray, &rxCommandCnt) > 0) {
// TODO: Check Status
break;
}
} else {
comm_timeouts[drive_number]++;
if (all_hardware_read) {
all_hardware_read = false;
std::cout << "[error] timeout in " << (int) receive_attempts << " communication cycle on drives";
}
std::cout << " " << (int) drive_number << "(" << port_names[drive_number].c_str() << ")";
break;
}
}
}
// If Hardware Panic, after receiving data, wait till the end of comm cycle and return.
if (hardware_panic) {
struct timespec delay;
delay.tv_nsec = 2000000;
delay.tv_sec = 0;
nanosleep(&delay, NULL);
return ret;
}
if (all_hardware_read) {
for (drive_number = 0; drive_number <= last_drive_number; drive_number++)
comm_timeouts[drive_number] = 0;
} else {
std::cout << std::endl;
}
// Inicjalizacja flag
robot_synchronized = true;
power_fault = false;
for (drive_number = 0; drive_number <= last_drive_number; drive_number++) {
// Wypelnienie pol odebranymi danymi
// NFComBuf.ReadDrivesPosition.data[] contains last received value
servo_data[drive_number].previous_absolute_position = servo_data[drive_number].current_absolute_position;
servo_data[drive_number].current_absolute_position = NFComBuf.ReadDrivesPosition.data[drive_number];
// Ustawienie flagi wlaczonej mocy
if ((NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_PowerStageFault)!= 0) {
power_fault = true;
}
// Ustawienie flagi synchronizacji
if ((NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_Synchronized)== 0) {
robot_synchronized = false;
}
// Sprawdzenie, czy wlasnie nastapila synchronizacja kolejnej osi
if (last_synchro_state[drive_number] == 0
&& (NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_Synchronized)!= 0) {
servo_data[drive_number].first_hardware_reads = FIRST_HARDWARE_READS_WITH_ZERO_INCREMENT;
last_synchro_state[drive_number] = 1;
}
// W pierwszych odczytach danych z napedu przyrost pozycji musi byc 0.
if ((servo_data[drive_number].first_hardware_reads > 0)) {
servo_data[drive_number].previous_absolute_position = servo_data[drive_number].current_absolute_position;
servo_data[drive_number].first_hardware_reads--;
}
// Sprawdzenie przyrostu pozycji enkodera
servo_data[drive_number].current_position_inc = (double) (servo_data[drive_number].current_absolute_position
- servo_data[drive_number].previous_absolute_position);
if ((robot_synchronized) && ((int) ridiculous_increment[drive_number] != 0)) {
/*if (drive_number == 0) {
std::cout << "inc: " << servo_data[drive_number].current_position_inc << " cur: "
<< servo_data[drive_number].current_absolute_position << " prev: "
<< servo_data[drive_number].previous_absolute_position << " time: " << boost::get_system_time()
<< std::endl;
}*/
if ((servo_data[drive_number].current_position_inc > ridiculous_increment[drive_number])
|| (servo_data[drive_number].current_position_inc < -ridiculous_increment[drive_number])) {
hardware_panic = true;
temp_message << "[error] ridiculous increment on drive " << (int) drive_number << ", "
<< port_names[drive_number].c_str() << ", c.cycle " << (int) receive_attempts << ": read = "
<< servo_data[drive_number].current_position_inc << ", max = "
<< ridiculous_increment[drive_number] << std::endl;
//master.msg->message(lib::FATAL_ERROR, temp_message.str());
std::cerr << temp_message.str() << std::cerr.flush();
}
}
// Sprawdzenie ograniczenia nadpradowego
if ((NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_Overcurrent)!= 0) {
if (error_msg_overcurrent == 0) {
//master.msg->message(lib::NON_FATAL_ERROR, "Overcurrent");
std::cout << "[error] overcurrent on drive " << (int) drive_number << ", "
<< port_names[drive_number].c_str() << ": read = "
<< NFComBuf.ReadDrivesCurrent.data[drive_number] << "mA" << std::endl;
error_msg_overcurrent++;
}
}
// Wykrywanie sekwencji timeoutow komunikacji
if (comm_timeouts[drive_number] >= MAX_COMM_TIMEOUTS) {
hardware_panic = true;
temp_message << "[error] multiple communication timeouts on drive " << (int) drive_number << "("
<< port_names[drive_number].c_str() << "): limit = " << MAX_COMM_TIMEOUTS << std::endl;
//master.msg->message(lib::FATAL_ERROR, temp_message.str());
std::cerr << temp_message.str() << std::cerr.flush();
}
}
//master.controller_state_edp_buf.is_synchronised = robot_synchronized;
//master.controller_state_edp_buf.robot_in_fault_state = power_fault;
if (power_fault) {
hardware_panic = true;
std::cout << "[error] power_fault" << std::endl;
if (error_msg_power_stage == 0) {
temp_message << "[error] power_fault" << std::endl;
std::cerr << temp_message.str() << std::cerr.flush();
//master.msg->message(lib::NON_FATAL_ERROR, "Wylaczono moc - robot zablokowany");
error_msg_power_stage++;
}
} else {
error_msg_power_stage = 0;
}
for (drive_number = 0; drive_number <= last_drive_number; drive_number++) {
if ((NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_LimitSwitchUp)!= 0)ret
|= (uint64_t) (UPPER_LIMIT_SWITCH << (5 * (drive_number))); // Zadzialal wylacznik "gorny" krancowy
if ((NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_LimitSwitchDown)!= 0)ret
|= (uint64_t) (LOWER_LIMIT_SWITCH << (5 * (drive_number))); // Zadzialal wylacznik "dolny" krancowy
if ((NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_EncoderIndexSignal)!= 0)ret
|= (uint64_t) (SYNCHRO_ZERO << (5 * (drive_number))); // Impuls zera rezolwera
if ((NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_Overcurrent)!= 0)ret
|= (uint64_t) (OVER_CURRENT << (5 * (drive_number))); // Przekroczenie dopuszczalnego pradu
if ((NFComBuf.ReadDrivesStatus.data[drive_number] & NF_DrivesStatus_SynchroSwitch)!= 0) {
if (synchro_switch_filter[drive_number] == synchro_switch_filter_th)
ret |= (uint64_t) (SYNCHRO_SWITCH_ON << (5 * (drive_number))); // Zadzialal wylacznik synchronizacji
else
synchro_switch_filter[drive_number]++;
} else {
synchro_switch_filter[drive_number] = 0;
}
}
if (status_disp_cnt++ == STATUS_DISP_T) {
status_disp_cnt = 0;
}
delay.tv_nsec = 2000000;
delay.tv_sec = 0;
nanosleep(&delay, NULL);
return ret;
}
//read motor current from communication buffer
int HI_moxa::set_parameter_now(int drive_number, const int parameter, ...)
{
struct timespec delay;
uint8_t setParamCommandCnt = 0;
uint8_t setParamCommandArray[10];
va_list newValue;
va_start(newValue, parameter);
switch (parameter)
{
case NF_COMMAND_SetDrivesMisc:
NFComBuf.SetDrivesMisc.data[drive_number] = (uint32_t) va_arg(newValue, int);
setParamCommandArray[setParamCommandCnt++] = NF_COMMAND_SetDrivesMisc;
break;
case NF_COMMAND_SetDrivesMaxCurrent:
NFComBuf.SetDrivesMaxCurrent.data[drive_number] = (int16_t) va_arg(newValue, int);
setParamCommandArray[setParamCommandCnt++] = NF_COMMAND_SetDrivesMaxCurrent;
break;
case NF_COMMAND_SetDrivesMode:
NFComBuf.SetDrivesMode.data[drive_number] = (uint8_t) va_arg(newValue, int);
setParamCommandArray[setParamCommandCnt++] = NF_COMMAND_SetDrivesMode;
break;
case NF_COMMAND_SetCurrentRegulator:
NFComBuf.SetCurrentRegulator.data[drive_number] = va_arg(newValue, NF_STRUCT_Regulator);
setParamCommandArray[setParamCommandCnt++] = NF_COMMAND_SetCurrentRegulator;
break;
default:
std::cout << "[error] HI_moxa::set_parameter_now() invalid parameter " << (int) parameter << std::endl;
return -1;
break;
}
va_end(newValue);
// Add Read Drive Status request
setParamCommandArray[setParamCommandCnt++] = NF_COMMAND_ReadDrivesStatus;
// Make command frame
txCnt = NF_MakeCommandFrame(&NFComBuf, txBuf + 5, (const uint8_t*) setParamCommandArray, setParamCommandCnt, drives_addresses[drive_number]);
// Clear communication request
setParamCommandCnt = 0;
for (int param_set_attempt = 0; param_set_attempt < MAX_PARAM_SET_ATTEMPTS; param_set_attempt++) {
// Send command frame
SerialPort[drive_number]->write(txBuf, txCnt + 5);
// hardware panic; do not print error information; do not wait for response
if (parameter == NF_COMMAND_SetDrivesMode && NFComBuf.SetDrivesMode.data[drive_number] == NF_DrivesMode_ERROR)
return 0;
// Give some time for a response to return
delay.tv_nsec = 1700000;
delay.tv_sec = 0;
nanosleep(&delay, NULL);
rxCnt = 0;
while (1) {
if (SerialPort[drive_number]->read(&(rxBuf[rxCnt]), 1) > 0 && (rxCnt < 255)) {
if (NF_Interpreter(&NFComBuf, rxBuf, &rxCnt, rxCommandArray, &rxCommandCnt) > 0) {
// TODO: Check status;
return 0;
}
} else {
std::cout << "[error] param (" << parameter << ") set ack timeout for drive (" << drive_number << ")"
<< std::endl;
break;
}
}
}
throw std::runtime_error("HI_Moxa: Unable to communicate with motor controllers. Try switching the power on.");
return 1;
}
uint64_t HI_moxa::write_hardware(void) {
static int error_msg_hardware_panic = 0;
uint64_t ret = 0;
uint8_t drive_number;
// static std::stringstream temp_message;
// If Hardware Panic, send PARAM_DRIVER_MODE_ERROR to motor drivers
if (hardware_panic) {
for (drive_number = 0; drive_number <= last_drive_number; drive_number++) {
// only set error parameter, do not wait for answer
servo_data[drive_number].commandCnt = 0;
NFComBuf.SetDrivesMode.data[drive_number] = NF_DrivesMode_ERROR;
servo_data[drive_number].commandArray[servo_data[drive_number].commandCnt++] =
NF_COMMAND_SetDrivesMode;
txCnt = NF_MakeCommandFrame(
&NFComBuf, txBuf,
(const uint8_t*) servo_data[drive_number].commandArray,
servo_data[drive_number].commandCnt, drives_addresses[drive_number]);
// Clear communication request
servo_data[drive_number].commandCnt = 0;
// Send command frame
SerialPort[drive_number]->write(txBuf, txCnt);
}
if (error_msg_hardware_panic == 0) {
std::cout << "[error] hardware panic" << std::endl;
error_msg_hardware_panic++;
}
// std::cout << temp_message.str() << std::endl;
/*
struct timespec delay;
delay.tv_nsec = 2000000;
delay.tv_sec = 0;
nanosleep(&delay, NULL);
*/
return ret;
} else {
// przygotowanie pod test zablokowania komunikacji po bledzie
if (all_hardware_read)
//if (1)
{
// Make command frames and send them to drives
for (drive_number = 0; drive_number <= last_drive_number;
drive_number++) {
// Set communication requests
// std::cout << "write drive_number: " << drive_number ;
servo_data[drive_number].commandArray[servo_data[drive_number]
.commandCnt++] =
NF_COMMAND_ReadDrivesPosition;
servo_data[drive_number].commandArray[servo_data[drive_number]
.commandCnt++] =
NF_COMMAND_ReadDrivesCurrent;
servo_data[drive_number].commandArray[servo_data[drive_number]
.commandCnt++] =
NF_COMMAND_ReadDrivesStatus;
// Make command frame
servo_data[drive_number].txCnt = NF_MakeCommandFrame(
&NFComBuf, servo_data[drive_number].txBuf + howMuchItSucks,
(const uint8_t*) servo_data[drive_number].commandArray,
servo_data[drive_number].commandCnt,
drives_addresses[drive_number]);
// Clear communication requests
servo_data[drive_number].commandCnt = 0;
}
for (drive_number = 0; drive_number <= last_drive_number;
drive_number++) {
// Send command frame
if (receiveFailCnt[drive_number] == 0)
SerialPort[drive_number]->write(
servo_data[drive_number].txBuf,
servo_data[drive_number].txCnt + howMuchItSucks);
#define SPN
#ifdef SPN
static int rwhprints = 18;
if (rwhprints) {
rwhprints--;
std::cout << "RWH: ";
for (int i = 0; i < servo_data[drive_number].txCnt + howMuchItSucks;
i++) {
std::cout << std::hex
<< (unsigned int) servo_data[drive_number].txBuf[i];
std::cout << " ";
}
std::cout << std::endl;
}
#endif
}
} else {
// std::cout << "write hardware !all_hardware_read " << std::endl;
}
}
ret = 1;
return ret;
}