|
@@ -58,25 +58,25 @@ int Canbus::Start() {
|
|
|
|
|
|
//judge whether the call is success.if pcan_handle=null,the call would be failed
|
|
|
if(pcan_handle){
|
|
|
- LOG(INFO)<<Name()<<"CAN Bus: "<<szDevNode<<" opened";
|
|
|
+ LOG(INFO)<<Name()<<": "<<szDevNode<<" opened";
|
|
|
errno = funCAN_VersionInfo(pcan_handle, chVersion);
|
|
|
if (!errno)
|
|
|
- LOG(INFO)<<Name()<<"CAN Bus: driver version ="<<chVersion;
|
|
|
+ LOG(INFO)<<Name()<<": driver version ="<<chVersion;
|
|
|
else {
|
|
|
- LOG(ERROR)<<Name()<<"CAN Bus: CAN_VersionInfo() error";
|
|
|
+ LOG(ERROR)<<Name()<<": CAN_VersionInfo() error";
|
|
|
}
|
|
|
if (unBaudrate) {
|
|
|
errno = funCAN_Init(pcan_handle, unBaudrate, nExtended);
|
|
|
if (errno) {
|
|
|
- LOG(ERROR)<<Name()<<"CAN Bus: CAN_Init()";
|
|
|
+ LOG(ERROR)<<Name()<<": CAN_Init()";
|
|
|
}
|
|
|
else
|
|
|
- LOG(INFO)<<"Device Info: " << szDevNode <<"; CAN_BAUD_1M; CAN_INIT_TYPE_ST";
|
|
|
+ LOG(INFO)<<" Device Info: " << szDevNode <<"; CAN_BAUD_1M; CAN_INIT_TYPE_ST";
|
|
|
}
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- LOG(INFO)<<Name()<<"CAN Bus: can't open "<<szDevNode;
|
|
|
+ LOG(INFO)<<Name()<<": can't open ."<<szDevNode;
|
|
|
}
|
|
|
|
|
|
ros::Rate loop_rate(10);
|
|
@@ -94,7 +94,7 @@ int Canbus::Start() {
|
|
|
|
|
|
|
|
|
void Canbus::OnTimer(const ros::TimerEvent &) {
|
|
|
- LOG(INFO)<< Name()<< "timer start ..";
|
|
|
+ LOG(INFO)<< Name()<< " timer start ..";
|
|
|
//ros::Publisher geometry_pub = node_handle_->advertise<geometry_msgs::Point>("geometry", 200);
|
|
|
|
|
|
MessageCoder::CanMsgPackage((void*)&status1,0x6F1,&canStatus1);
|
|
@@ -111,7 +111,7 @@ void Canbus::Stop() {
|
|
|
if (pcan_handle) {
|
|
|
funCAN_Close(pcan_handle);
|
|
|
}
|
|
|
- LOG(INFO)<<Name()<<"CAN Bus: finished. ";
|
|
|
+ LOG(INFO)<<Name()<<": closed. ";
|
|
|
}
|
|
|
|
|
|
|
|
@@ -122,7 +122,7 @@ int Canbus::read_loop()
|
|
|
__u32 status;
|
|
|
|
|
|
if (funLINUX_CAN_Read(pcan_handle, &msg)) {
|
|
|
- LOG(ERROR)<<Name()<<"receive: LINUX_CAN_Read() error.";
|
|
|
+ LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
|
|
|
return errno;
|
|
|
}
|
|
|
// deal receive data
|
|
@@ -131,10 +131,10 @@ int Canbus::read_loop()
|
|
|
status = funCAN_Status(pcan_handle);
|
|
|
if ((int)status < 0) {
|
|
|
errno = funnGetLastError();
|
|
|
- LOG(ERROR)<<Name()<<"receive: CAN_Status() error";
|
|
|
+ LOG(ERROR)<<Name()<<" receive: CAN_Status() error.";
|
|
|
return errno;
|
|
|
}
|
|
|
- LOG(INFO)<<Name()<<"receive: pending CAN status "<<(__u16)status<<" read.";
|
|
|
+ LOG(INFO)<<Name()<<" receive: pending CAN status "<<(__u16)status<<" read.";
|
|
|
read_data(&(msg.Msg));
|
|
|
}
|
|
|
return ROBOT_SUCCESS;
|
|
@@ -142,6 +142,7 @@ int Canbus::read_loop()
|
|
|
|
|
|
int Canbus::read_data(TPCANMsg *pMsgBuff)
|
|
|
{
|
|
|
+ LOG(INFO)<<Name()<<" Receive CanData : ID: "<<pMsgBuff->ID<<",Data: "<<pMsgBuff->DATA;
|
|
|
if(pMsgBuff->ID == 0x1A1)
|
|
|
{
|
|
|
T_OBD_ACC_PSNG tOBDAccPsng = *((T_OBD_ACC_PSNG*)MessageCoder::CanMsgAnalyse(0x1A1,pMsgBuff->DATA));
|
|
@@ -184,10 +185,11 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
|
|
|
//canData.set_data(reinterpret_cast<const char*>(pMsgBuff->DATA));
|
|
|
std_msgs::String ros_msg;
|
|
|
std::stringstream ss;
|
|
|
- ss << "Id: " << pMsgBuff->ID << ", Data: " << pMsgBuff->DATA;
|
|
|
+ ss << "Id:" << pMsgBuff->ID << ",Data:" << pMsgBuff->DATA;
|
|
|
ros_msg.data = ss.str();
|
|
|
ros::Publisher canData_pub = node_handle_->advertise<std_msgs::String>(robot::common::CanDataAnalysis, 200);
|
|
|
canData_pub.publish(ros_msg);
|
|
|
+ LOG(INFO)<<Name()<<" Publish CanData :"<<ros_msg;
|
|
|
}
|
|
|
|
|
|
return ROBOT_SUCCESS;
|
|
@@ -195,8 +197,9 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
|
|
|
|
|
|
int Canbus::write_data(TPCANMsg *pMsgBuff)
|
|
|
{
|
|
|
+ LOG(INFO)<<Name()<<" send CanData : ID: "<<pMsgBuff->ID<<",Data:"<<pMsgBuff->DATA;
|
|
|
if (funCAN_Write(pcan_handle, pMsgBuff)){
|
|
|
- LOG(ERROR)<<Name()<< "write: LINUX_CAN_write_timeout error. ";
|
|
|
+ LOG(ERROR)<<Name()<< " write: LINUX_CAN_write_timeout error. ";
|
|
|
return errno;
|
|
|
}
|
|
|
return ROBOT_SUCCESS;
|