socketcan_to_topic.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2016, Ivor Wanders
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions are met:
6  * * Redistributions of source code must retain the above copyright notice,
7  * this list of conditions and the following disclaimer.
8  * * Redistributions in binary form must reproduce the above copyright
9  * notice, this list of conditions and the following disclaimer in the
10  * documentation and/or other materials provided with the distribution.
11  * * Neither the name of the copyright holder nor the names of its
12  * contributors may be used to endorse or promote products derived from
13  * this software without specific prior written permission.
14  *
15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25  * POSSIBILITY OF SUCH DAMAGE.
26  */
27 
30 #include <can_msgs/Frame.h>
31 #include <string>
32 
33 namespace can {
35  XmlRpc::XmlRpcValue t(ct);
36  try{ // to read as integer
37  uint32_t id = static_cast<int>(t);
38  return tofilter(id);
39  }
40  catch(...){ // read as string
41  return tofilter(static_cast<std::string>(t));
42  }
43 }
44 }
45 
46 namespace socketcan_bridge
47 {
48  SocketCANToTopic::SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
50  {
51  can_topic_ = nh->advertise<can_msgs::Frame>("received_messages", 10);
52  driver_ = driver;
53  };
54 
55  void SocketCANToTopic::setup()
56  {
57  // register handler for frames and state changes.
58  frame_listener_ = driver_->createMsgListener(
59  can::CommInterface::FrameDelegate(this, &SocketCANToTopic::frameCallback));
60 
61  state_listener_ = driver_->createStateListener(
62  can::StateInterface::StateDelegate(this, &SocketCANToTopic::stateCallback));
63  };
64 
65  void SocketCANToTopic::setup(const can::FilteredFrameListener::FilterVector &filters){
66  frame_listener_.reset(new can::FilteredFrameListener(driver_,
67  can::CommInterface::FrameDelegate(this, &SocketCANToTopic::frameCallback),
68  filters));
69 
70  state_listener_ = driver_->createStateListener(
71  can::StateInterface::StateDelegate(this, &SocketCANToTopic::stateCallback));
72  }
73 
74  void SocketCANToTopic::setup(XmlRpc::XmlRpcValue filters) {
75  setup(can::tofilters(filters));
76  }
77  void SocketCANToTopic::setup(ros::NodeHandle nh) {
78  XmlRpc::XmlRpcValue filters;
79  if(nh.getParam("can_ids", filters)) return setup(filters);
80  return setup();
81  }
82 
83 
84  void SocketCANToTopic::frameCallback(const can::Frame& f)
85  {
86  // ROS_DEBUG("Message came in: %s", can::tostring(f, true).c_str());
87  if (!f.isValid())
88  {
89  ROS_ERROR("Invalid frame from SocketCAN: id: %#04x, length: %d, is_extended: %d, is_error: %d, is_rtr: %d",
90  f.id, f.dlc, f.is_extended, f.is_error, f.is_rtr);
91  return;
92  }
93  else
94  {
95  if (f.is_error)
96  {
97  // can::tostring cannot be used for dlc > 8 frames. It causes an crash
98  // due to usage of boost::array for the data array. The should always work.
99  ROS_WARN("Received frame is error: %s", can::tostring(f, true).c_str());
100  }
101  }
102 
103  can_msgs::Frame msg;
104  // converts the can::Frame (socketcan.h) to can_msgs::Frame (ROS msg)
106 
107  msg.header.frame_id = ""; // empty frame is the de-facto standard for no frame.
108  msg.header.stamp = ros::Time::now();
109 
110  can_topic_.publish(msg);
111  };
112 
113 
114  void SocketCANToTopic::stateCallback(const can::State & s)
115  {
116  std::string err;
117  driver_->translateError(s.internal_error, err);
118  if (!s.internal_error)
119  {
120  ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
121  }
122  else
123  {
124  ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
125  }
126  };
127 }; // namespace socketcan_bridge
FilteredFrameListener::FilterVector tofilters(const T &v)
std::string tostring(const Header &h, bool lc)
unsigned int is_error
FrameFilterSharedPtr tofilter(const T &ct)
#define ROS_WARN(...)
std::vector< FrameFilterSharedPtr > FilterVector
unsigned int is_extended
#define ROS_INFO(...)
void convertSocketCANToMessage(const can::Frame &f, can_msgs::Frame &m)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool isValid() const
bool getParam(const std::string &key, std::string &s) const
static Time now()
unsigned int internal_error
unsigned char dlc
unsigned int id
#define ROS_ERROR(...)
unsigned int is_rtr
boost::system::error_code error_code


socketcan_bridge
Author(s): Ivor Wanders
autogenerated on Sat May 4 2019 02:40:49