CanExtractor.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015-2020, Dataspeed Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Dataspeed Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include "CanExtractor.h"
36 
38 
39 CanExtractor::CanExtractor(const std::string &dbc_file, bool offline, bool expand, bool unknown) :
40  dbc_(dbc_file), offline_(offline)
41 {
42  bag_open_ = false;
43  expand_ = expand;
44  unknown_ = unknown;
45 }
46 
47 CanExtractor::CanExtractor(const std::vector<std::string> &dbc_file, bool offline, bool expand, bool unknown) :
48  dbc_(dbc_file), offline_(offline)
49 {
50  bag_open_ = false;
51  expand_ = expand;
52  unknown_ = unknown;
53 }
54 
55 uint64_t CanExtractor::unsignedSignalData(uint64_t raw_data, const RosCanSigStruct& sig_props)
56 {
57  // C++ facts! Bitshifting a value by greater than or equal to the number of bits in the left expression's type
58  // has an undefined result! ((uint32_t) 1) << 32) may have unexpected results on different compilers
59  // (we had an issue where the value was equal to 1 rather than all zeros).
60  uint64_t mask;
61  if (sig_props.length >= 64) {
62  mask = ((uint64_t) -1); // 0xFFFFFFFFFFFFFFFF
63  } else {
64  // Make sure the 1 is an unsigned long!
65  mask = (((uint64_t)1) << sig_props.length) - 1;
66  }
67 
68  if (sig_props.order == INTEL) {
69  // Little Endian
70  return (raw_data >> sig_props.start_bit) & mask;
71  } else {
72  // Big Endian
73  int intel_start_bit = 56 - 8 * (sig_props.start_bit / 8) + (sig_props.start_bit % 8) - (sig_props.length - 1);
74  return (__builtin_bswap64(raw_data) >> intel_start_bit) & mask;
75  }
76 }
77 
78 int64_t CanExtractor::signedSignalData(uint64_t raw_data, const RosCanSigStruct& sig_props)
79 {
80  uint64_t mask;
81  if (sig_props.length >= 64) {
82  mask = ((uint64_t) -1); // 0xFFFFFFFFFFFFFFFF
83  } else {
84  // Make sure the 1 is an unsigned long!
85  mask = (((uint64_t)1) << sig_props.length) - 1;
86  }
87 
88  int64_t val = unsignedSignalData(raw_data, sig_props);
89 
90  if (val & (1 << (sig_props.length - 1))) {
91  val |= ~mask;
92  }
93 
94  return val;
95 }
96 
97 template<class T>
98 T CanExtractor::buildMsg(const RosCanSigStruct& info, const uint64_t& data, bool sign)
99 {
100  T msg;
101  if (sign) {
102  msg.data = (info.factor * signedSignalData(data, info)) + info.offset;
103  } else {
104  msg.data = (info.factor * unsignedSignalData(data, info)) + info.offset;
105  }
106  return msg;
107 }
108 
109 int CanExtractor::getAppropriateSize(const RosCanSigStruct& sig_props, bool output_signed)
110 {
111  if (sig_props.length >= 64) {
112  return 64;
113  }
114 
115  int64_t max_val;
116  int64_t min_val;
117  if ((sig_props.sign == SIGNED)) {
118  max_val = (((int64_t)1 << (sig_props.length - 1)) - 1);
119  min_val = -((int64_t)1 << (sig_props.length - 1));
120  } else {
121  max_val = (((int64_t)1 << sig_props.length) - 1);
122  min_val = 0;
123  }
124  max_val = max_val * (int64_t)sig_props.factor + (int64_t)sig_props.offset;
125  min_val = min_val * (int64_t)sig_props.factor + (int64_t)sig_props.offset;
126  if (max_val < min_val) {
127  std::swap(min_val, max_val);
128  }
129 
130  if (output_signed) {
131  if ((INT8_MIN <= min_val) && (max_val <= INT8_MAX)) {
132  return 8;
133  } else if ((INT16_MIN <= min_val) && (max_val <= INT16_MAX)) {
134  return 16;
135  } else if ((INT32_MIN <= min_val) && (max_val <= INT32_MAX)) {
136  return 32;
137  } else {
138  return 64;
139  }
140  } else {
141  if (max_val <= UINT8_MAX) {
142  return 8;
143  } else if (max_val <= UINT16_MAX) {
144  return 16;
145  } else if (max_val <= UINT32_MAX) {
146  return 32;
147  } else {
148  return 64;
149  }
150  }
151 }
152 
154 {
155  if (msgs_.find(can_msg.id) == msgs_.end()) {
156  for (DBCIterator::const_iterator it = dbc_.begin(); it < dbc_.end(); it++) {
157  if (it->getId() == can_msg.id) {
158  can_msg.msg_name = it->getName();
159 
160  for (Message::const_iterator m_it = it->begin(); m_it < it->end(); m_it++) {
161  RosCanSigStruct new_sig;
162  new_sig.factor = m_it->getFactor();
163  new_sig.length = m_it->getLength();
164  new_sig.maximum = m_it->getMaximum();
165  new_sig.minimum = m_it->getMinimum();
166  new_sig.offset = m_it->getOffset();
167  new_sig.order = m_it->getByteOrder();
168  new_sig.sig_name = m_it->getName();
169  new_sig.sign = m_it->getSign();
170  new_sig.start_bit = m_it->getStartbit();
171  new_sig.multiplexor = m_it->getMultiplexor();
172  new_sig.multiplexNum = m_it->getMultiplexedNumber();
173  can_msg.sigs.push_back(new_sig);
174  }
175 
176  msgs_[can_msg.id] = can_msg;
177  return true;
178  }
179  }
180 
181  if (unknown_msgs_.find(can_msg.id) == unknown_msgs_.end()) {
182  unknown_msgs_[can_msg.id] = 0;
183  if (unknown_) {
184  std::stringstream ss;
185  ss << "x" << std::hex << std::setfill('0') << std::setw(3) << can_msg.id;
186  can_msg.msg_name = ss.str();
187  msgs_[can_msg.id] = can_msg;
188  return true;
189  }
190  ROS_WARN("Received unknown CAN message with ID = 0x%X", can_msg.id);
191  }
192  } else {
193  can_msg = msgs_[can_msg.id];
194  }
195 
196  return false;
197 }
198 
200 {
201  ros::NodeHandle nh_msg(nh, info.msg_name);
202 
203  info.message_pub = nh.advertise<can_msgs::Frame>(info.msg_name, 1);
204 
205  if (expand_) {
206  ROS_DEBUG("Initializing publishers for %zu signals...", info.sigs.size());
207  for (size_t i=0; i<info.sigs.size(); i++){
208  registerCanSignalPublisher(info.sigs[i], nh_msg);
209  }
210  }
211 
212  msgs_[info.id] = info;
213 }
214 
215 bool CanExtractor::openBag(const std::string &fname, rosbag::compression::CompressionType compression)
216 {
217  if (!bag_open_) {
218  // Save the desired file name, to actually open the bag later for writing.
219  bag_.setCompression(compression);
220  bag_fname_ = fname;
221  return true;
222  }
223  return false;
224 }
225 
227 {
228  if (bag_open_) {
229  bag_.close();
230  bag_open_ = false;
231  return true;
232  }
233  return false;
234 }
235 
237 {
238  const uint32_t QUEUE_SIZE = 10;
239  if (info.length == 1) {
240  info.sig_pub = nh.advertise<std_msgs::Bool>(info.sig_name, QUEUE_SIZE);
241  } else if ((fmod(info.factor, 1.0) != 0) || (fmod(info.offset, 1.0) != 0)) {
242  info.sig_pub = nh.advertise<std_msgs::Float64>(info.sig_name, QUEUE_SIZE);
243  } else {
244  if ((info.sign == SIGNED) || (info.offset < 0) || (info.factor < 0)) {
245  switch (getAppropriateSize(info, true)) {
246  case 8: info.sig_pub = nh.advertise<std_msgs::Int8 >(info.sig_name, QUEUE_SIZE); break;
247  case 16: info.sig_pub = nh.advertise<std_msgs::Int16>(info.sig_name, QUEUE_SIZE); break;
248  case 32: info.sig_pub = nh.advertise<std_msgs::Int32>(info.sig_name, QUEUE_SIZE); break;
249  case 64: info.sig_pub = nh.advertise<std_msgs::Int64>(info.sig_name, QUEUE_SIZE); break;
250  }
251  } else {
252  switch (getAppropriateSize(info, false)) {
253  case 8: info.sig_pub = nh.advertise<std_msgs::UInt8 >(info.sig_name, QUEUE_SIZE); break;
254  case 16: info.sig_pub = nh.advertise<std_msgs::UInt16>(info.sig_name, QUEUE_SIZE); break;
255  case 32: info.sig_pub = nh.advertise<std_msgs::UInt32>(info.sig_name, QUEUE_SIZE); break;
256  case 64: info.sig_pub = nh.advertise<std_msgs::UInt64>(info.sig_name, QUEUE_SIZE); break;
257  }
258  }
259  }
260 }
261 
262 template<class T>
263 void CanExtractor::writeToBag(const std::string& frame, const ros::Time& stamp, const T& msg) {
264  // Check the bag file is open before writing.
265  if (!bag_open_) {
266  ROS_DEBUG("Opening bag file for writing...");
267  bag_open_ = true;
269  }
270  bag_.write(frame, stamp, msg);
271 }
272 
273 template<class T>
274 void CanExtractor::pubCanSig(const RosCanMsgStruct& info, const T& sig_msg, const ros::Time& stamp, size_t i) {
275  ROS_DEBUG(" Publishing value (%s): %f", info.sigs[i].sig_name.c_str(), (double)sig_msg.data);
276  if (i < info.sigs.size()) {
277  if (offline_) {
278  writeToBag(info.msg_name + "/" + info.sigs[i].sig_name, stamp, sig_msg);
279  } else {
280  info.sigs[i].sig_pub.publish(sig_msg);
281  }
282  }
283 }
284 
285 void CanExtractor::pubCanMsg(const RosCanMsgStruct& info, const can_msgs::Frame& msg, const ros::Time& stamp) {
286  if (offline_) {
287  writeToBag(info.msg_name, stamp, msg);
288  } else {
289  info.message_pub.publish(msg);
290  }
291 }
292 
293 void CanExtractor::pubMessage(const can_msgs::Frame& msg, const ros::Time &stamp)
294 {
295  // Check for valid message information
296  const uint32_t id = msg.id | (msg.is_extended ? 0x80000000 : 0x00000000);
297  if (msgs_.find(id) == msgs_.end()) {
298  ROS_WARN("Skipping unknown message ID: 0x%03X", id);
299  return;
300  }
301  const RosCanMsgStruct &info = msgs_[id];
302  const uint64_t data = *((uint64_t*)&msg.data[0]);
303 
304  // Re-publish CAN message on named topic
305  pubCanMsg(info, msg, stamp);
306 
307  // Only continue if individual signals are to be expanded and published
308  if (!expand_) return;
309 
310  // Search for the multiplexor value, if any.
311  unsigned short multiplexorValue = -1;
312  for (size_t i = 0; i < info.sigs.size(); i++) {
313  if (info.sigs[i].multiplexor == MULTIPLEXOR) {
314  multiplexorValue = unsignedSignalData(data, info.sigs[i]);
315  break;
316  }
317  }
318 
319  // Publish signals on their own topics
320  for (size_t i = 0; i < info.sigs.size(); i++) {
321 
322  // Handle multiplexed signals
323  ROS_DEBUG("MSG Name: %s", info.sigs[i].sig_pub.getTopic().c_str());
324 
325  if (info.sigs[i].multiplexor == MULTIPLEXED) {
326  if (info.sigs[i].multiplexNum != multiplexorValue) {
327  ROS_DEBUG(" Skipping multiplexed value...");
328  continue; // Else, skip this iteration of the loop.
329  } // If sigs[i].multiplexNum == multiplexorValue, it should be published.
330  } // If sigs[i].multiplexor equals MULTIPLEXOR or NONE, it's fine to publish for all messages.
331 
332  // Publish various message types
333  if (info.sigs[i].length == 1) {
334  pubCanSig(info, buildMsg<std_msgs::Bool>(info.sigs[i], data, false), stamp, i);
335  } else if ((fmod(info.sigs[i].factor, 1.0) != 0) || fmod(info.sigs[i].offset, 1.0) != 0) {
336  pubCanSig(info, buildMsg<std_msgs::Float64>(info.sigs[i], data, info.sigs[i].sign == SIGNED), stamp, i);
337  } else {
338  if ((info.sigs[i].sign == SIGNED) || (info.sigs[i].offset < 0) || (info.sigs[i].factor < 0)) {
339  if (info.sigs[i].sign == SIGNED) {
340  switch (getAppropriateSize(info.sigs[i], true)) {
341  case 8: pubCanSig(info, buildMsg<std_msgs::Int8 >(info.sigs[i], data, true), stamp, i); break;
342  case 16: pubCanSig(info, buildMsg<std_msgs::Int16>(info.sigs[i], data, true), stamp, i); break;
343  case 32: pubCanSig(info, buildMsg<std_msgs::Int32>(info.sigs[i], data, true), stamp, i); break;
344  case 64: pubCanSig(info, buildMsg<std_msgs::Int64>(info.sigs[i], data, true), stamp, i); break;
345  }
346  } else {
347  switch (getAppropriateSize(info.sigs[i], true)) {
348  case 8: pubCanSig(info, buildMsg<std_msgs::Int8 >(info.sigs[i], data, false), stamp, i); break;
349  case 16: pubCanSig(info, buildMsg<std_msgs::Int16>(info.sigs[i], data, false), stamp, i); break;
350  case 32: pubCanSig(info, buildMsg<std_msgs::Int32>(info.sigs[i], data, false), stamp, i); break;
351  case 64: pubCanSig(info, buildMsg<std_msgs::Int64>(info.sigs[i], data, false), stamp, i); break;
352  }
353  }
354  } else {
355  switch (getAppropriateSize(info.sigs[i], false)) {
356  case 8: pubCanSig(info, buildMsg<std_msgs::UInt8 >(info.sigs[i], data, false), stamp, i); break;
357  case 16: pubCanSig(info, buildMsg<std_msgs::UInt16>(info.sigs[i], data, false), stamp, i); break;
358  case 32: pubCanSig(info, buildMsg<std_msgs::UInt32>(info.sigs[i], data, false), stamp, i); break;
359  case 64: pubCanSig(info, buildMsg<std_msgs::UInt64>(info.sigs[i], data, false), stamp, i); break;
360  }
361  }
362  }
363  }
364 }
365 
366 } // namespace dataspeed_can_tools
367 
void pubCanSig(const RosCanMsgStruct &info, const T &sig_msg, const ros::Time &stamp, size_t i)
const_iterator end() const
Definition: DbcIterator.hpp:67
void publish(const boost::shared_ptr< M > &message) const
void open(std::string const &filename, uint32_t mode=bagmode::Read)
void writeToBag(const std::string &frame, const ros::Time &stamp, const T &msg)
static void registerCanSignalPublisher(RosCanSigStruct &info, ros::NodeHandle &nh)
static T buildMsg(const RosCanSigStruct &info, const uint64_t &data, bool sign)
std::map< uint32_t, RosCanMsgStruct > msgs_
Definition: CanExtractor.h:114
static int64_t signedSignalData(uint64_t raw_data, const RosCanSigStruct &sig_props)
#define ROS_WARN(...)
void close()
static uint64_t unsignedSignalData(uint64_t raw_data, const RosCanSigStruct &sig_props)
std::map< uint32_t, int > unknown_msgs_
Definition: CanExtractor.h:115
void setCompression(CompressionType compression)
void pubCanMsg(const RosCanMsgStruct &info, const can_msgs::Frame &msg, const ros::Time &stamp)
messages_t::const_iterator const_iterator
Definition: DbcIterator.hpp:54
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool openBag(const std::string &fname, rosbag::compression::CompressionType compression=rosbag::compression::Uncompressed)
static int getAppropriateSize(const RosCanSigStruct &sig_props, bool output_signed)
const_iterator begin() const
Definition: DbcIterator.hpp:66
signals_t::const_iterator const_iterator
Definition: DbcMessage.hpp:65
void pubMessage(const can_msgs::Frame &msg, const ros::Time &stamp=ros::Time(0))
CanExtractor(const std::string &dbc_file, bool offline, bool expand=true, bool unknown=false)
bool getMessage(RosCanMsgStruct &can_msg)
std::vector< RosCanSigStruct > sigs
Definition: CanExtractor.h:78
void initPublishers(RosCanMsgStruct &info, ros::NodeHandle &nh)
void write(std::string const &topic, ros::MessageEvent< T > const &event)
#define ROS_DEBUG(...)


dataspeed_can_tools
Author(s): Micho Radovnikovich
autogenerated on Thu Jul 9 2020 03:41:59