00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "PdsNode.h"
00036 #include <dataspeed_pds_can/dispatch.h>
00037
00038 namespace dataspeed_pds_can
00039 {
00040
00041 PdsNode::PdsNode(ros::NodeHandle &node, ros::NodeHandle &priv_nh)
00042 : sync_can_master_(8, boost::bind(&PdsNode::recvSync, this, _1, MASTER), ID_STATUS1_MASTER, ID_STATUS2_MASTER, ID_CURRENT1_MASTER, ID_CURRENT2_MASTER, ID_CURRENT3_MASTER)
00043 , sync_can_slave1_(8, boost::bind(&PdsNode::recvSync, this, _1, SLAVE1), ID_STATUS1_SLAVE1, ID_STATUS2_SLAVE1, ID_CURRENT1_SLAVE1, ID_CURRENT2_SLAVE1, ID_CURRENT3_SLAVE1)
00044 , sync_can_slave2_(8, boost::bind(&PdsNode::recvSync, this, _1, SLAVE2), ID_STATUS1_SLAVE2, ID_STATUS2_SLAVE2, ID_CURRENT1_SLAVE2, ID_CURRENT2_SLAVE2, ID_CURRENT3_SLAVE2)
00045 , sync_can_slave3_(8, boost::bind(&PdsNode::recvSync, this, _1, SLAVE3), ID_STATUS1_SLAVE3, ID_STATUS2_SLAVE3, ID_CURRENT1_SLAVE3, ID_CURRENT2_SLAVE3, ID_CURRENT3_SLAVE3)
00046 {
00047
00048 sync_ros_slave1_ = new message_filters::Synchronizer<SyncPolicy1>(SyncPolicy1(4), sync_msg_master_, sync_msg_slave1_);
00049 sync_ros_slave2_ = new message_filters::Synchronizer<SyncPolicy2>(SyncPolicy2(4), sync_msg_master_, sync_msg_slave1_, sync_msg_slave2_);
00050 sync_ros_slave3_ = new message_filters::Synchronizer<SyncPolicy3>(SyncPolicy3(4), sync_msg_master_, sync_msg_slave1_, sync_msg_slave2_, sync_msg_slave3_);
00051 sync_ros_slave1_->registerCallback(boost::bind(&PdsNode::recvSyncSlave1, this, _1, _2));
00052 sync_ros_slave2_->registerCallback(boost::bind(&PdsNode::recvSyncSlave2, this, _1, _2, _3));
00053 sync_ros_slave3_->registerCallback(boost::bind(&PdsNode::recvSyncSlave3, this, _1, _2, _3, _4));
00054
00055
00056 const ros::Duration SYNC_50_MS(0.016);
00057 for (unsigned int i = 0; i < 5; i++) {
00058 sync_can_master_.setInterMessageLowerBound(i, SYNC_50_MS);
00059 sync_can_slave1_.setInterMessageLowerBound(i, SYNC_50_MS);
00060 sync_can_slave2_.setInterMessageLowerBound(i, SYNC_50_MS);
00061 sync_can_slave3_.setInterMessageLowerBound(i, SYNC_50_MS);
00062 }
00063 sync_ros_slave1_->setInterMessageLowerBound(0, SYNC_50_MS);
00064 sync_ros_slave1_->setInterMessageLowerBound(1, SYNC_50_MS);
00065 sync_ros_slave2_->setInterMessageLowerBound(0, SYNC_50_MS);
00066 sync_ros_slave2_->setInterMessageLowerBound(1, SYNC_50_MS);
00067 sync_ros_slave2_->setInterMessageLowerBound(2, SYNC_50_MS);
00068 sync_ros_slave3_->setInterMessageLowerBound(0, SYNC_50_MS);
00069 sync_ros_slave3_->setInterMessageLowerBound(1, SYNC_50_MS);
00070 sync_ros_slave3_->setInterMessageLowerBound(2, SYNC_50_MS);
00071 sync_ros_slave3_->setInterMessageLowerBound(3, SYNC_50_MS);
00072
00073
00074 pub_status_ = node.advertise<dataspeed_pds_msgs::Status>("status", 10);
00075 pub_can_ = node.advertise<can_msgs::Frame>("can_tx", 10);
00076
00077
00078 sub_relay_ = node.subscribe("relay", 10, &PdsNode::recvRelay, this, ros::TransportHints().tcpNoDelay(true));
00079 sub_mode_ = node.subscribe("mode", 10, &PdsNode::recvMode, this, ros::TransportHints().tcpNoDelay(true));
00080 sub_script_ = node.subscribe("script", 10, &PdsNode::recvScript, this, ros::TransportHints().tcpNoDelay(true));
00081 sub_can_ = node.subscribe("can_rx", 80, &PdsNode::recvCAN, this, ros::TransportHints().tcpNoDelay(true));
00082 }
00083 PdsNode::~PdsNode()
00084 {
00085 if (sync_ros_slave1_) {
00086 delete sync_ros_slave1_;
00087 sync_ros_slave1_ = NULL;
00088 }
00089 if (sync_ros_slave2_) {
00090 delete sync_ros_slave2_;
00091 sync_ros_slave2_ = NULL;
00092 }
00093 if (sync_ros_slave3_) {
00094 delete sync_ros_slave3_;
00095 sync_ros_slave3_ = NULL;
00096 }
00097 }
00098
00099 void PdsNode::recvCAN(const can_msgs::Frame::ConstPtr& msg)
00100 {
00101 if (!msg->is_rtr && !msg->is_error && !msg->is_extended) {
00102 sync_can_master_.processMsg(msg);
00103 sync_can_slave1_.processMsg(msg);
00104 sync_can_slave2_.processMsg(msg);
00105 sync_can_slave3_.processMsg(msg);
00106 switch (msg->id) {
00107 case ID_REQUEST:
00108 case ID_MODE:
00109 case ID_SCRIPT:
00110 break;
00111 case ID_RESERVED1:
00112 case ID_RESERVED2:
00113 case ID_RESERVED3:
00114 case ID_RESERVED4:
00115 break;
00116 case ID_STATUS1_MASTER:
00117 case ID_STATUS2_MASTER:
00118 case ID_CURRENT1_MASTER:
00119 case ID_CURRENT2_MASTER:
00120 case ID_CURRENT3_MASTER:
00121 break;
00122 case ID_STATUS1_SLAVE1:
00123 case ID_STATUS2_SLAVE1:
00124 case ID_CURRENT1_SLAVE1:
00125 case ID_CURRENT2_SLAVE1:
00126 case ID_CURRENT3_SLAVE1:
00127 break;
00128 case ID_STATUS1_SLAVE2:
00129 case ID_STATUS2_SLAVE2:
00130 case ID_CURRENT1_SLAVE2:
00131 case ID_CURRENT2_SLAVE2:
00132 case ID_CURRENT3_SLAVE2:
00133 break;
00134 case ID_STATUS1_SLAVE3:
00135 case ID_STATUS2_SLAVE3:
00136 case ID_CURRENT1_SLAVE3:
00137 case ID_CURRENT2_SLAVE3:
00138 case ID_CURRENT3_SLAVE3:
00139 break;
00140 default:
00141 #if 0
00142 ROS_WARN("Unknown message ID: %1$d (0x%1$01x)", msg->id);
00143 #endif
00144 break;
00145 }
00146 }
00147 }
00148
00149 void PdsNode::recvRelay(const dataspeed_pds_msgs::Relay::ConstPtr &msg)
00150 {
00151 can_msgs::Frame out;
00152 ((MsgRelay*)out.data.elems)->channel = msg->channel;
00153 ((MsgRelay*)out.data.elems)->request = msg->request;
00154 out.id = ID_REQUEST;
00155 out.dlc = sizeof(MsgRelay);
00156 out.is_extended = false;
00157 pub_can_.publish(out);
00158 }
00159
00160 void PdsNode::recvMode(const dataspeed_pds_msgs::Mode::ConstPtr &msg)
00161 {
00162 can_msgs::Frame out;
00163 ((MsgMode*)out.data.elems)->mode = msg->mode;
00164 out.id = ID_MODE;
00165 out.dlc = sizeof(MsgMode);
00166 out.is_extended = false;
00167 pub_can_.publish(out);
00168 }
00169
00170 void PdsNode::recvScript(const dataspeed_pds_msgs::Script::ConstPtr &msg)
00171 {
00172 can_msgs::Frame out;
00173 MsgScript *ptr = (MsgScript*)out.data.elems;
00174 ((MsgScript*)out.data.elems)->script = msg->script;
00175 out.id = ID_SCRIPT;
00176 out.dlc = sizeof(MsgScript);
00177 out.is_extended = false;
00178 pub_can_.publish(out);
00179 }
00180
00181 void PdsNode::recvSync(const std::vector<can_msgs::Frame::ConstPtr> &msgs, UnitId id)
00182 {
00183 ROS_ASSERT((MASTER <= id) && (id <= SLAVE3));
00184 ROS_ASSERT(msgs.size() == 5);
00185 ROS_ASSERT(msgs[0]->id == ID_STATUS1_MASTER + id);
00186 ROS_ASSERT(msgs[1]->id == ID_STATUS2_MASTER + id);
00187 ROS_ASSERT(msgs[2]->id == ID_CURRENT1_MASTER + id);
00188 ROS_ASSERT(msgs[3]->id == ID_CURRENT2_MASTER + id);
00189 ROS_ASSERT(msgs[4]->id == ID_CURRENT3_MASTER + id);
00190 if ((msgs[0]->dlc >= sizeof(MsgStatus1))
00191 && (msgs[1]->dlc >= sizeof(MsgStatus2))
00192 && (msgs[2]->dlc >= sizeof(MsgCurrent))
00193 && (msgs[3]->dlc >= sizeof(MsgCurrent))
00194 && (msgs[4]->dlc >= sizeof(MsgCurrent))) {
00195 const MsgStatus1 *ptrS1 = (const MsgStatus1*)msgs[0]->data.elems;
00196 const MsgStatus2 *ptrS2 = (const MsgStatus2*)msgs[1]->data.elems;
00197 const MsgCurrent *ptrC1 = (const MsgCurrent*)msgs[2]->data.elems;
00198 const MsgCurrent *ptrC2 = (const MsgCurrent*)msgs[3]->data.elems;
00199 const MsgCurrent *ptrC3 = (const MsgCurrent*)msgs[4]->data.elems;
00200
00201
00202 dataspeed_pds_msgs::Status msg;
00203 msg.header.stamp = msgs[0]->header.stamp;
00204 msg.mode.mode = ptrS1->mode;
00205 msg.script.script = ptrS1->script;
00206 msg.chan.resize(12);
00207 msg.chan[ 0].status = ptrS1->status_01;
00208 msg.chan[ 1].status = ptrS1->status_02;
00209 msg.chan[ 2].status = ptrS1->status_03;
00210 msg.chan[ 3].status = ptrS1->status_04;
00211 msg.chan[ 4].status = ptrS1->status_05;
00212 msg.chan[ 5].status = ptrS1->status_06;
00213 msg.chan[ 6].status = ptrS1->status_07;
00214 msg.chan[ 7].status = ptrS1->status_08;
00215 msg.chan[ 8].status = ptrS1->status_09;
00216 msg.chan[ 9].status = ptrS1->status_10;
00217 msg.chan[10].status = ptrS1->status_11;
00218 msg.chan[11].status = ptrS1->status_12;
00219 msg.chan[ 0].current = bytesToAmperes(ptrC1->current_01);
00220 msg.chan[ 1].current = bytesToAmperes(ptrC1->current_02);
00221 msg.chan[ 2].current = bytesToAmperes(ptrC1->current_03);
00222 msg.chan[ 3].current = bytesToAmperes(ptrC1->current_04);
00223 msg.chan[ 4].current = bytesToAmperes(ptrC2->current_05);
00224 msg.chan[ 5].current = bytesToAmperes(ptrC2->current_06);
00225 msg.chan[ 6].current = bytesToAmperes(ptrC2->current_07);
00226 msg.chan[ 7].current = bytesToAmperes(ptrC2->current_08);
00227 msg.chan[ 8].current = bytesToAmperes(ptrC3->current_09);
00228 msg.chan[ 9].current = bytesToAmperes(ptrC3->current_10);
00229 msg.chan[10].current = bytesToAmperes(ptrC3->current_11);
00230 msg.chan[11].current = bytesToAmperes(ptrC3->current_12);
00231 msg.master.inverter.request = ptrS1->inverter_request;
00232 msg.master.inverter.status = ptrS1->inverter_status;
00233 msg.master.inverter.overload = ptrS1->inverter_overload;
00234 msg.master.inverter.overtemp = ptrS1->inverter_overtemp;
00235 msg.master.temp.internal = bytesToCelsius(ptrS2->board_temp);
00236 msg.master.temp.external = bytesToCelsius(ptrS2->thermocouple_temp);
00237 msg.master.voltage = bytesToVoltage(ptrS2->voltage);
00238
00239
00240 const dataspeed_pds_msgs::Status::ConstPtr ptr(new dataspeed_pds_msgs::Status(msg));
00241 const ros::Time now = ros::Time::now();
00242 switch (id) {
00243 case MASTER:
00244 sync_msg_master_.add(ptr);
00245 if ((now - stamp_slave1_) > TIMEOUT) {
00246 pub_status_.publish(msg);
00247 }
00248 break;
00249 case SLAVE1:
00250 stamp_slave1_ = now;
00251 sync_msg_slave1_.add(ptr);
00252 break;
00253 case SLAVE2:
00254 stamp_slave2_ = now;
00255 sync_msg_slave2_.add(ptr);
00256 break;
00257 case SLAVE3:
00258 stamp_slave3_ = now;
00259 sync_msg_slave3_.add(ptr);
00260 break;
00261 }
00262 }
00263 }
00264
00265 void PdsNode::recvSyncSlave1(const SyncPtr& master, const SyncPtr& slave1)
00266 {
00267 if ((ros::Time::now() - stamp_slave2_) > TIMEOUT) {
00268 dataspeed_pds_msgs::Status msg = *master;
00269 msg.chan.insert(msg.chan.end(), slave1->chan.begin(), slave1->chan.end());
00270 msg.slave.push_back(slave1->master);
00271 pub_status_.publish(msg);
00272 }
00273 }
00274 void PdsNode::recvSyncSlave2(const SyncPtr& master, const SyncPtr& slave1, const SyncPtr& slave2)
00275 {
00276 if ((ros::Time::now() - stamp_slave3_) > TIMEOUT) {
00277 dataspeed_pds_msgs::Status msg = *master;
00278 msg.chan.insert(msg.chan.end(), slave1->chan.begin(), slave1->chan.end());
00279 msg.chan.insert(msg.chan.end(), slave2->chan.begin(), slave2->chan.end());
00280 msg.slave.push_back(slave1->master);
00281 msg.slave.push_back(slave2->master);
00282 pub_status_.publish(msg);
00283 }
00284 }
00285 void PdsNode::recvSyncSlave3(const SyncPtr& master, const SyncPtr& slave1, const SyncPtr& slave2, const SyncPtr& slave3)
00286 {
00287 if (1) {
00288 dataspeed_pds_msgs::Status msg = *master;
00289 msg.chan.insert(msg.chan.end(), slave1->chan.begin(), slave1->chan.end());
00290 msg.chan.insert(msg.chan.end(), slave2->chan.begin(), slave2->chan.end());
00291 msg.chan.insert(msg.chan.end(), slave3->chan.begin(), slave3->chan.end());
00292 msg.slave.push_back(slave1->master);
00293 msg.slave.push_back(slave2->master);
00294 msg.slave.push_back(slave3->master);
00295 pub_status_.publish(msg);
00296 }
00297 }
00298
00299 }
00300