static void readMsgPair (int burstNr, int msgNr, int handle1, int handle2)
{
canStatus stat;
long errorCounter = 0;
for(;;) {
stat = canReadWait(handle1, &id1, &data1, &dlc1, &flag1, &(unsigned long)t1, 4000);
if (stat != canOK) {
printSomeStuff();
printf("burstNr %d, msgNr %d\n", burstNr, msgNr);
Check("canRead() failed on handle1", stat);
}
if (flag1 & (canMSGERR_HW_OVERRUN | canMSGERR_SW_OVERRUN)) {
printSomeStuff();
printf("burstNr %d, msgNr %d\n", burstNr, msgNr);
Error("canRead() reported an overrun on handle1, flag1=0x%x", flag1);
} else if (flag1 & canMSG_ERROR_FRAME) {
h1Errors++;
errorCounter++;
printf(". 0x%02x ",flag1);
} else {
break;
}
if(errorCounter > 1000) {
Error("readMsgPair() A found too many error frames",errorCounter);
}
}
errorCounter = 0;
for(;;) {
stat = canReadWait(handle2, &id2, &data2, &dlc2, &flag2, &(unsigned long)t2, 4000);
if (stat != canOK) {
printSomeStuff();
printf("burstNr %d, msgNr %d\n", burstNr, msgNr);
Check("canRead() failed on handle2", stat);
}
if (flag2 & (canMSGERR_HW_OVERRUN | canMSGERR_SW_OVERRUN)) {
printSomeStuff();
printf("burstNr %d, msgNr %d\n", burstNr, msgNr);
Error("canRead() reported an overrun on handle2, flag2=0x%x", flag2);
} else if (flag2 & canMSG_ERROR_FRAME) {
h2Errors++;
errorCounter++;
printf("* 0x%02x ",flag2);
} else {
break;
}
if(errorCounter > 1000) {
Error("readMsgPair() B found too many error frames",errorCounter);
}
}
}
/**
* CAN_HANDLES must have value >=1 while CANLIB wants handles >= 0
* so fd0 needs to be decremented before use.
*
*/
UNS8 canReceive_driver(CAN_HANDLE fd0, Message *m)
{
canStatus retval = canOK;
unsigned flags = 0;
unsigned long timeStamp;
fd0--;
/* checking for input message (blocking) */
retval = canReadWait((int)fd0, (long*)&m->cob_id, &m->data, (unsigned*)&m->len, &flags, &timeStamp, -1);
if (retval != canOK)
{
fprintf(stderr, "canReceive_driver (Kvaser) : canReadWait() error, cob_id=%08X, len=%u, flags=%08X, returned value = %d\n",
m->cob_id, m->len, flags, retval);
canClose((int)fd0);
return retval;
}
m->rtr = 0;
if (flags & canMSG_RTR)
{
m->rtr = 1;
}
if (flags & canMSG_EXT)
{
/* TODO: is it correct to set this info in cob_id? */
m->cob_id |= 0x20000000;
}
//fprintf(stderr, "canReceive_driver (Kvaser) : canReadWait() received packet, cob_id=%08X, len=%u, flags=%08X, timestamp=%d returned value = %d\n",
// m->cob_id, m->len, flags, timeStamp, retval);
return retval;
}
/* Reads a packet. Looks like we can just read from one channel.
* writes it into msg (must already be allocated), and returns id, dlc, and flags
*
* returns: # bytes in msg if a packet is read, 0 if no packet available, and
* negative on error
*/
int
CANIOKvaser::ReadPacket(CanPacket *pkt, int channel)
{
int ret=0;
long unsigned time;
if((ret = canReadWait(channels[channel], &(pkt->id), &(pkt->msg),
&(pkt->dlc), &(pkt->flags), &time, -1)) < 0)
{
// either no messages or an error
if(ret == canERR_NOMSG)
return 0;
else
return ret;
}
return pkt->dlc;
}
static bool dbg_kavaser_can_recv(uint32 *ch, uint32 *canid, uint32 *ex_canid, uint8 *data, uint8 *dlc)
{
unsigned int flag = 0;
unsigned int out_canid;
unsigned int out_dlc;
unsigned long time;
static unsigned int count= 0;
count++;
if ((count % 10000) != 0) {
return FALSE;
}
dbg_kvaser.status = canReadWait(dbg_kvaser.handle, (long*)&out_canid, (void*)data, &out_dlc, (unsigned int*)&flag, &time, 0);
if (dbg_kvaser.status != canOK) {
//printf("dbg_kavaser_can_recv:canReadWait err=%d\n", dbg_kvaser.status);
return FALSE;
}
#if 1 /* for debug */
{
int i;
*ch = 1;
*canid = out_canid;
*ex_canid = 0xFFFFFFFF;//TODO
*dlc = out_dlc;
printf("##RCV_CAN::ch=%u\n", *ch);
printf("##RCV_CNA:canid=0x%x\n", *canid);
printf("##RCV_CAN:ex_canid=0x%x\n", *ex_canid);
printf("##RCV_CNA:dlc=%u\n", *dlc);
printf("##");
for (i = 0; i < *dlc; i++) {
printf("0x%02x ", data[i]);
}
printf("\n");
fflush(stdout);
}
#endif
return TRUE;
}
int main(int argc, char *argv[]) {
// Setup CAN and ROS. Then loop forever processing received CAN messages, and publishing
// info on ROS topics as needed. Also subcribed to ROS topic for commands.
int ret = -1;
long id;
unsigned char msg[8];
unsigned int dlc;
unsigned int flag;
unsigned long t;
int channel = 0;
int bitrate = BAUD_250K;
///////////////
// ROS setup //
///////////////
ros::init(argc, argv, "globe_epas");
ros::NodeHandle n;
ros::Publisher globe_epas_command_mode_pub = n.advertise<globe_epas::globe_epas_cmd>("report/command_mode", 1000);
ros::Publisher globe_epas_inc_position_pub = n.advertise<std_msgs::Float64>("report/inc_position", 1000);
ros::Publisher globe_epas_current_pub = n.advertise<std_msgs::Float64>("report/current", 1000);
ros::Publisher globe_epas_velocity_pub = n.advertise<std_msgs::Float64>("report/velocity", 1000);
ros::Publisher globe_epas_unit_temp_pub = n.advertise<std_msgs::Float64>("report/unit_temp", 1000);
ros::Publisher globe_epas_encoder_temp_pub = n.advertise<std_msgs::Float64>("report/encoder_temp", 1000);
ros::Publisher globe_epas_torque_input_pub = n.advertise<std_msgs::Float64>("report/torque_input", 1000);
ros::Publisher globe_epas_torque_output_pub = n.advertise<std_msgs::Float64>("report/torque_output", 1000);
ros::Subscriber globe_epas_set_command_mode_sub = n.subscribe("command/set_command_mode", 1000, callback_set_command_mode);
ros::Subscriber globe_epas_set_current_sub = n.subscribe("command/set_current", 1000, callback_set_current);
ros::Subscriber globe_epas_set_speed_sub = n.subscribe("command/set_speed", 1000, callback_set_speed);
ros::Subscriber globe_epas_set_position_sub = n.subscribe("command/set_position", 1000, callback_set_position);
ros::Subscriber globe_epas_set_position_with_speed_limit_sub = n.subscribe("command/set_position_with_speed_limit", 1000, callback_set_position_with_speed_limit);
ros::Subscriber globe_epas_set_inc_encoder_value_sub = n.subscribe("command/set_inc_encoder_value", 1000, callback_set_inc_encoder_value);
// ros::Rate loop_rate(10);
///////////////
// CAN setup //
///////////////
// Use sighand as our signal handler
signal(SIGALRM, sighand);
signal(SIGINT, sighand);
alarm(1);
// Allow signals to interrupt syscalls (in canReadBlock)
siginterrupt(SIGINT, 1);
// Open channels, set parameters and go on bus
h = canOpenChannel(channel, canOPEN_REQUIRE_EXTENDED);
if(h < 0) {
ROS_INFO("canOpenChannel %d failed\n", channel);
return -1;
}
canSetBusParams(h, bitrate, 4, 3, 1, 1, 0);
canSetBusOutputControl(h, canDRIVER_NORMAL);
canBusOn(h);
// Main loop, waiting for one of the three Globe EPAS feedback messages
while(!willExit) {
do {
ret = canReadWait(h, &id, &msg, &dlc, &flag, &t, -1);
switch (ret) {
case 0:
if(id==0x18ff0113) {
double inc_position = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])*360.0/pow(2,20);
double current = ((msg[7]<<24) + (msg[6]<<16) + (msg[5]<<8) + msg[4])/pow(2,20);
std_msgs::Float64 pub_msg;
pub_msg.data=inc_position;
globe_epas_inc_position_pub.publish(pub_msg);
pub_msg.data=current;
globe_epas_current_pub.publish(pub_msg);
} else if(id==0x18ff0213) {
double velocity = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])*360.0/pow(2,20)/gear_ratio;
double unit_temp = msg[4]-40.0;
double encoder_temp = msg[5]-40.0;
std_msgs::Float64 pub_msg;
pub_msg.data=velocity;
globe_epas_velocity_pub.publish(pub_msg);
pub_msg.data=unit_temp;
globe_epas_unit_temp_pub.publish(pub_msg);
pub_msg.data=encoder_temp;
globe_epas_encoder_temp_pub.publish(pub_msg);
} else if(id==0x18ff0313) {
double torque_input = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])/pow(2,20);
double torque_output = ((msg[7]<<24) + (msg[6]<<16) + (msg[5]<<8) + msg[4])/pow(2,20);
std_msgs::Float64 pub_msg;
pub_msg.data=torque_input;
globe_epas_torque_input_pub.publish(pub_msg);
pub_msg.data=torque_output;
globe_epas_torque_output_pub.publish(pub_msg);
}
break;
case canERR_NOMSG:
break;
default:
perror("canReadBlock error");
break;
}
globe_epas::globe_epas_cmd pub_msg;
pub_msg.command_mode = command_mode;
globe_epas_command_mode_pub.publish(pub_msg);
ros::spinOnce();
} while((ret==canOK)&&(ros::ok()));
willExit=1;
}
canClose(h);
sighand(SIGALRM);
return 0;
}
int canReadWait_ (int handle, long int w) { return (int) canReadWait (handle, &id, msg, &dlc, &flags, &time, w); }
int main (int argc, char *argv[])
{
canHandle h;
int ret = -1;
long id;
unsigned char msg[8];
unsigned int dlc;
unsigned int flag;
unsigned long t;
int channel = 0;
int bitrate = BAUD_500K;
int j;
kvaser::CANPacket candat;
ros::init(argc, argv, "can_listener");
ros::NodeHandle n;
ros::Publisher can_pub = n.advertise<kvaser::CANPacket>("can_raw", 10);
errno = 0;
if (argc != 2 || (channel = atoi(argv[1]), errno) != 0) {
printf("usage %s channel\n", argv[0]);
exit(1);
} else {
printf("Reading messages on channel %d\n", channel);
}
/* Use sighand as our signal handler */
signal(SIGALRM, sighand);
signal(SIGINT, sighand);
alarm(1);
/* Allow signals to interrupt syscalls(in canReadBlock) */
siginterrupt(SIGINT, 1);
/* Open channels, parameters and go on bus */
h = canOpenChannel(channel, canOPEN_EXCLUSIVE | canOPEN_REQUIRE_EXTENDED);
if (h < 0) {
printf("canOpenChannel %d failed\n", channel);
return -1;
}
canSetBusParams(h, bitrate, 4, 3, 1, 1, 0);
canSetBusOutputControl(h, canDRIVER_NORMAL);
canBusOn(h);
i = 0;
// while (!willExit) {
while(ros::ok()){
do {
ret = canReadWait(h, &id, &msg, &dlc, &flag, &t, -1);
switch (ret) {
case 0:
printf("(%d) id:%ld dlc:%d data: ", i, id, dlc);
if (dlc > 8) {
dlc = 8;
}
for (j = 0; j < dlc; j++){
printf("%2.2x ", msg[j]);
candat.dat[j]=msg[j];
}
printf(" flags:0x%x time:%ld\n", flag, t);
candat.count =i;
candat.time=t;
candat.id = id;
candat.len=dlc;
candat.header.stamp=ros::Time::now();
can_pub.publish(candat);
ros::spinOnce();
i++;
if (last_time == 0) {
last_time = time(0);
} else if (time(0) > last_time) {
last_time = time(0);
if (i != last) {
printf("rx : %d total: %d\n", i - last, i);
}
last = i;
}
break;
case canERR_NOMSG:
break;
default:
perror("canReadBlock error");
break;
}
} while (ret == canOK);
willExit = 1;
}
canClose(h);
sighand(SIGALRM);
printf("Ready\n");
return 0;
}
