00001
00002
00003
00004
00005
00006
00007
00008 #include <thread>
00009 #include <mutex>
00010 #include <chrono>
00011
00012 #include <ros/ros.h>
00013 #include <kvaser_interface.h>
00014 #include <can_msgs/Frame.h>
00015
00016 using namespace AS::CAN;
00017
00018 int bit_rate = 500000;
00019 int hardware_id = 0;
00020 int circuit_id = 0;
00021 bool global_keep_going = true;
00022 std::mutex keep_going_mut;
00023 KvaserCan can_reader, can_writer;
00024 ros::Publisher can_tx_pub;
00025
00026 void can_read()
00027 {
00028 long id;
00029 uint8_t msg[8];
00030 unsigned int size;
00031 bool extended;
00032 unsigned long t;
00033
00034 const std::chrono::milliseconds loop_pause = std::chrono::milliseconds(10);
00035 bool keep_going = true;
00036
00037
00038 keep_going_mut.lock();
00039 keep_going = global_keep_going;
00040 keep_going_mut.unlock();
00041
00042 return_statuses ret;
00043
00044 while (keep_going)
00045 {
00046 std::chrono::system_clock::time_point next_time = std::chrono::system_clock::now();
00047 next_time += loop_pause;
00048
00049 if (!can_reader.is_open())
00050 {
00051 ret = can_reader.open(hardware_id, circuit_id, bit_rate, false);
00052
00053 if (ret != OK)
00054 ROS_ERROR_THROTTLE(0.5, "Kvaser CAN Interface - Error opening reader: %d - %s", ret, return_status_desc(ret).c_str());
00055 }
00056 else
00057 {
00058 while ((ret = can_reader.read(&id, msg, &size, &extended, &t)) == OK)
00059 {
00060 can_msgs::Frame can_pub_msg;
00061 can_pub_msg.header.frame_id = "0";
00062 can_pub_msg.id = id;
00063 can_pub_msg.dlc = size;
00064 std::copy(msg, msg + 8, can_pub_msg.data.begin());
00065 can_pub_msg.header.stamp = ros::Time::now();
00066 can_tx_pub.publish(can_pub_msg);
00067 }
00068
00069 if (ret != NO_MESSAGES_RECEIVED)
00070 ROS_WARN_THROTTLE(0.5, "Kvaser CAN Interface - Error reading CAN message: %d - %s", ret, return_status_desc(ret).c_str());
00071 }
00072
00073 std::this_thread::sleep_until(next_time);
00074
00075
00076 keep_going_mut.lock();
00077 keep_going = global_keep_going;
00078 keep_going_mut.unlock();
00079 }
00080
00081 if (can_reader.is_open())
00082 {
00083 ret = can_reader.close();
00084
00085 if (ret != OK)
00086 ROS_ERROR_THROTTLE(0.5, "Kvaser CAN Interface - Error closing reader: %d - %s", ret, return_status_desc(ret).c_str());
00087 }
00088 }
00089
00090 void can_rx_callback(const can_msgs::Frame::ConstPtr& msg)
00091 {
00092 return_statuses ret;
00093
00094 if (!can_writer.is_open())
00095 {
00096
00097 ret = can_writer.open(hardware_id, circuit_id, bit_rate, false);
00098
00099 if (ret != OK)
00100 {
00101 ROS_ERROR_THROTTLE(0.5, "Kvaser CAN Interface - Error opening writer: %d - %s", ret, return_status_desc(ret).c_str());
00102 }
00103 }
00104
00105 if (can_writer.is_open())
00106 {
00107 ret = can_writer.write(msg->id, const_cast<unsigned char*>(&(msg->data[0])), msg->dlc, msg->is_extended);
00108
00109 if (ret != OK)
00110 ROS_WARN_THROTTLE(0.5, "Kvaser CAN Interface - CAN send error: %d - %s", ret, return_status_desc(ret).c_str());
00111 }
00112 }
00113
00114 int main(int argc, char** argv)
00115 {
00116 bool exit = false;
00117
00118
00119 ros::init(argc, argv, "kvaser_can_bridge");
00120 ros::NodeHandle n;
00121 ros::NodeHandle priv("~");
00122 ros::AsyncSpinner spinner(1);
00123
00124 can_tx_pub = n.advertise<can_msgs::Frame>("can_tx", 500);
00125
00126 ros::Subscriber can_rx_sub = n.subscribe("can_rx", 500, can_rx_callback);
00127
00128
00129 while (ros::Time::now().nsec == 0);
00130
00131 if (priv.getParam("can_hardware_id", hardware_id))
00132 {
00133 ROS_INFO("Kvaser CAN Interface - Got hardware_id: %d", hardware_id);
00134
00135 if (hardware_id <= 0)
00136 {
00137 ROS_ERROR("Kvaser CAN Interface - CAN hardware ID is invalid.");
00138 exit = true;
00139 }
00140 }
00141
00142 if (priv.getParam("can_circuit_id", circuit_id))
00143 {
00144 ROS_INFO("Kvaser CAN Interface - Got can_circuit_id: %d", circuit_id);
00145
00146 if (circuit_id < 0)
00147 {
00148 ROS_ERROR("Kvaser CAN Interface - Circuit ID is invalid.");
00149 exit = true;
00150 }
00151 }
00152
00153 if (priv.getParam("can_bit_rate", bit_rate))
00154 {
00155 ROS_INFO("Kvaser CAN Interface - Got bit_rate: %d", bit_rate);
00156
00157 if (bit_rate < 0)
00158 {
00159 ROS_ERROR("Kvaser CAN Interface - Bit Rate is invalid.");
00160 exit = true;
00161 }
00162 }
00163
00164 if (exit)
00165 return 0;
00166
00167
00168 std::thread can_read_thread(can_read);
00169
00170 spinner.start();
00171
00172 ros::waitForShutdown();
00173
00174 return_statuses ret = can_writer.close();
00175
00176 if (ret != OK)
00177 ROS_ERROR("Kvaser CAN Interface - Error closing writer: %d - %s", ret, return_status_desc(ret).c_str());
00178
00179 keep_going_mut.lock();
00180 global_keep_going = false;
00181 keep_going_mut.unlock();
00182
00183 can_read_thread.join();
00184
00185 return 0;
00186 }