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
00037 namespace dataspeed_pds_lcm
00038 {
00039
00040 PdsNode::PdsNode(ros::NodeHandle &nh, ros::NodeHandle &priv_nh, lcm::LCM *lcm)
00041 {
00042
00043 sync_ros_slave1_ = new message_filters::Synchronizer<SyncPolicy1>(SyncPolicy1(5), sync_msg_master_, sync_msg_slave1_);
00044 sync_ros_slave2_ = new message_filters::Synchronizer<SyncPolicy2>(SyncPolicy2(5), sync_msg_master_, sync_msg_slave1_, sync_msg_slave2_);
00045 sync_ros_slave3_ = new message_filters::Synchronizer<SyncPolicy3>(SyncPolicy3(5), sync_msg_master_, sync_msg_slave1_, sync_msg_slave2_, sync_msg_slave3_);
00046 sync_ros_slave1_->registerCallback(boost::bind(&PdsNode::recvSyncSlave1, this, _1, _2));
00047 sync_ros_slave2_->registerCallback(boost::bind(&PdsNode::recvSyncSlave2, this, _1, _2, _3));
00048 sync_ros_slave3_->registerCallback(boost::bind(&PdsNode::recvSyncSlave3, this, _1, _2, _3, _4));
00049
00050
00051 const ros::Duration SYNC_20_MS(0.006);
00052 sync_ros_slave1_->setInterMessageLowerBound(0, SYNC_20_MS);
00053 sync_ros_slave1_->setInterMessageLowerBound(1, SYNC_20_MS);
00054 sync_ros_slave2_->setInterMessageLowerBound(0, SYNC_20_MS);
00055 sync_ros_slave2_->setInterMessageLowerBound(1, SYNC_20_MS);
00056 sync_ros_slave2_->setInterMessageLowerBound(2, SYNC_20_MS);
00057 sync_ros_slave3_->setInterMessageLowerBound(0, SYNC_20_MS);
00058 sync_ros_slave3_->setInterMessageLowerBound(1, SYNC_20_MS);
00059 sync_ros_slave3_->setInterMessageLowerBound(2, SYNC_20_MS);
00060 sync_ros_slave3_->setInterMessageLowerBound(3, SYNC_20_MS);
00061
00062
00063 pub_status_ = nh.advertise<dataspeed_pds_msgs::Status>("status", 10);
00064
00065
00066 sub_relay_ = nh.subscribe("relay", 10, &PdsNode::recvRelay, this, ros::TransportHints().tcpNoDelay(true));
00067 sub_mode_ = nh.subscribe("mode", 10, &PdsNode::recvMode, this, ros::TransportHints().tcpNoDelay(true));
00068 sub_script_ = nh.subscribe("script", 10, &PdsNode::recvScript, this, ros::TransportHints().tcpNoDelay(true));
00069
00070
00071 lcm_ = *lcm;
00072
00073
00074 lcm_.subscribe("STATUS", &PdsNode::lcmRecvStatus, this);
00075 }
00076 PdsNode::~PdsNode()
00077 {
00078 if (sync_ros_slave1_) {
00079 delete sync_ros_slave1_;
00080 sync_ros_slave1_ = NULL;
00081 }
00082 if (sync_ros_slave2_) {
00083 delete sync_ros_slave2_;
00084 sync_ros_slave2_ = NULL;
00085 }
00086 if (sync_ros_slave3_) {
00087 delete sync_ros_slave3_;
00088 sync_ros_slave3_ = NULL;
00089 }
00090 }
00091
00092 void PdsNode::lcmRecvStatus(const lcm::ReceiveBuffer* buf, const std::string &chan, const status_t *lcm) {
00093 #if 0
00094 ROS_INFO("Received LCM message from channel '%s'", chan.c_str());
00095 #endif
00096
00097
00098 const ros::Time now = ros::Time::now();
00099
00100
00101 dataspeed_pds_msgs::Status msg;
00102 msg.header.stamp = now;
00103 msg.mode.mode = lcm->mode;
00104 msg.script.script = 0;
00105 msg.chan.resize(12);
00106 for (size_t i = 0; i < 12; i++) {
00107 msg.chan[i].current = lcm->current[i];
00108 msg.chan[i].status = lcm->status[i];
00109 }
00110 msg.master.inverter.request = (lcm->inverter_status & (1 << 0)) ? true : false;
00111 msg.master.inverter.status = (lcm->inverter_status & (1 << 1)) ? true : false;
00112 msg.master.inverter.overload = (lcm->inverter_status & (1 << 2)) ? true : false;
00113 msg.master.inverter.overtemp = (lcm->inverter_status & (1 << 3)) ? true : false;
00114 msg.master.temp.internal = lcm->board_temp;
00115 msg.master.temp.external = lcm->thermocouple;
00116 msg.master.voltage = lcm->voltage;
00117
00118
00119 const dataspeed_pds_msgs::Status::ConstPtr ptr(new dataspeed_pds_msgs::Status(msg));
00120 switch (lcm->unit_id) {
00121 case MASTER:
00122 sync_msg_master_.add(ptr);
00123 if ((now - stamp_slave1_) > TIMEOUT) {
00124 pub_status_.publish(msg);
00125 }
00126 break;
00127 case SLAVE1:
00128 stamp_slave1_ = now;
00129 sync_msg_slave1_.add(ptr);
00130 break;
00131 case SLAVE2:
00132 stamp_slave2_ = now;
00133 sync_msg_slave2_.add(ptr);
00134 break;
00135 case SLAVE3:
00136 stamp_slave3_ = now;
00137 sync_msg_slave3_.add(ptr);
00138 break;
00139 }
00140 }
00141
00142 void PdsNode::recvRelay(const dataspeed_pds_msgs::Relay::ConstPtr &msg)
00143 {
00144 relay_request_t out;
00145 out.channel = msg->channel;
00146 out.request = msg->request;
00147 lcm_.publish("RELAY", &out);
00148 }
00149
00150 void PdsNode::recvMode(const dataspeed_pds_msgs::Mode::ConstPtr &msg)
00151 {
00152 mode_t out;
00153 out.mode = msg->mode;
00154 lcm_.publish("MODE", &out);
00155 }
00156
00157 void PdsNode::recvScript(const dataspeed_pds_msgs::Script::ConstPtr &msg)
00158 {
00159 script_request_t out;
00160 out.script = msg->script;
00161 lcm_.publish("SCRIPT", &out);
00162 }
00163
00164 void PdsNode::recvSyncSlave1(const SyncPtr& master, const SyncPtr& slave1)
00165 {
00166 if ((ros::Time::now() - stamp_slave2_) > TIMEOUT) {
00167 dataspeed_pds_msgs::Status msg = *master;
00168 msg.chan.insert(msg.chan.end(), slave1->chan.begin(), slave1->chan.end());
00169 msg.slave.push_back(slave1->master);
00170 pub_status_.publish(msg);
00171 }
00172 }
00173 void PdsNode::recvSyncSlave2(const SyncPtr& master, const SyncPtr& slave1, const SyncPtr& slave2)
00174 {
00175 if ((ros::Time::now() - stamp_slave3_) > TIMEOUT) {
00176 dataspeed_pds_msgs::Status msg = *master;
00177 msg.chan.insert(msg.chan.end(), slave1->chan.begin(), slave1->chan.end());
00178 msg.chan.insert(msg.chan.end(), slave2->chan.begin(), slave2->chan.end());
00179 msg.slave.push_back(slave1->master);
00180 msg.slave.push_back(slave2->master);
00181 pub_status_.publish(msg);
00182 }
00183 }
00184 void PdsNode::recvSyncSlave3(const SyncPtr& master, const SyncPtr& slave1, const SyncPtr& slave2, const SyncPtr& slave3)
00185 {
00186 if (1) {
00187 dataspeed_pds_msgs::Status msg = *master;
00188 msg.chan.insert(msg.chan.end(), slave1->chan.begin(), slave1->chan.end());
00189 msg.chan.insert(msg.chan.end(), slave2->chan.begin(), slave2->chan.end());
00190 msg.chan.insert(msg.chan.end(), slave3->chan.begin(), slave3->chan.end());
00191 msg.slave.push_back(slave1->master);
00192 msg.slave.push_back(slave2->master);
00193 msg.slave.push_back(slave3->master);
00194 pub_status_.publish(msg);
00195 }
00196 }
00197
00198 }
00199