to_socketcan_test.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  */
29 
30 #include <can_msgs/Frame.h>
33 
34 #include <gtest/gtest.h>
35 #include <ros/ros.h>
36 #include <list>
37 
39 {
40  public:
41  std::list<can::Frame> frames;
42 
44 
45  void frameCallback(const can::Frame& f)
46  {
47  frames.push_back(f);
48  }
49 };
50 
51 TEST(TopicToSocketCANTest, checkCorrectData)
52 {
53  ros::NodeHandle nh(""), nh_param("~");
54 
55  // create the dummy interface
56  can::DummyInterfaceSharedPtr driver_ = boost::make_shared<can::DummyInterface>(true);
57 
58  // start the to topic bridge.
59  socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver_);
60  to_socketcan_bridge.setup();
61 
62  // init the driver to test stateListener (not checked automatically).
63  driver_->init("string_not_used", true);
64 
65  // register for messages on received_messages.
66  ros::Publisher publisher_ = nh.advertise<can_msgs::Frame>("sent_messages", 10);
67 
68  // create a frame collector.
69  frameCollector frame_collector_;
70 
71  // driver->createMsgListener(&frameCallback);
72  can::FrameListenerConstSharedPtr frame_listener_ = driver_->createMsgListener(
74 
75  // create a message
76  can_msgs::Frame msg;
77  msg.is_extended = true;
78  msg.is_rtr = false;
79  msg.is_error = false;
80  msg.id = 0x1337;
81  msg.dlc = 8;
82  for (uint8_t i=0; i < msg.dlc; i++)
83  {
84  msg.data[i] = i;
85  }
86 
87  msg.header.frame_id = "0"; // "0" for no frame.
88  msg.header.stamp = ros::Time::now();
89 
90  // send the can_frame::Frame message to the sent_messages topic.
91  publisher_.publish(msg);
92 
93  // give some time for the interface some time to process the message
94  ros::WallDuration(1.0).sleep();
95  ros::spinOnce();
96 
97  can_msgs::Frame received;
98  can::Frame f = frame_collector_.frames.back();
100 
101  EXPECT_EQ(received.id, msg.id);
102  EXPECT_EQ(received.dlc, msg.dlc);
103  EXPECT_EQ(received.is_extended, msg.is_extended);
104  EXPECT_EQ(received.is_rtr, msg.is_rtr);
105  EXPECT_EQ(received.is_error, msg.is_error);
106  EXPECT_EQ(received.data, msg.data);
107 }
108 
109 TEST(TopicToSocketCANTest, checkInvalidFrameHandling)
110 {
111  // - tries to send a non-extended frame with an id larger than 11 bits.
112  // that should not be sent.
113  // - verifies that sending one larger than 11 bits actually works.
114  // - tries sending a message with a dlc > 8 bytes, this should not be sent.
115  // sending with 8 bytes is verified by the checkCorrectData testcase.
116 
117  ros::NodeHandle nh(""), nh_param("~");
118 
119  // create the dummy interface
120  can::DummyInterfaceSharedPtr driver_ = boost::make_shared<can::DummyInterface>(true);
121 
122  // start the to topic bridge.
123  socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver_);
124  to_socketcan_bridge.setup();
125 
126  // register for messages on received_messages.
127  ros::Publisher publisher_ = nh.advertise<can_msgs::Frame>("sent_messages", 10);
128 
129  // create a frame collector.
130  frameCollector frame_collector_;
131 
132  // add callback to the dummy interface.
133  can::FrameListenerConstSharedPtr frame_listener_ = driver_->createMsgListener(
135 
136  // create a message
137  can_msgs::Frame msg;
138  msg.is_extended = false;
139  msg.id = (1<<11)+1; // this is an illegal CAN packet... should not be sent.
140  msg.header.frame_id = "0"; // "0" for no frame.
141  msg.header.stamp = ros::Time::now();
142 
143  // send the can_frame::Frame message to the sent_messages topic.
144  publisher_.publish(msg);
145 
146  // give some time for the interface some time to process the message
147  ros::WallDuration(1.0).sleep();
148  ros::spinOnce();
149  EXPECT_EQ(frame_collector_.frames.size(), 0);
150 
151  msg.is_extended = true;
152  msg.id = (1<<11)+1; // now it should be alright.
153  // send the can_frame::Frame message to the sent_messages topic.
154  publisher_.publish(msg);
155  ros::WallDuration(1.0).sleep();
156  ros::spinOnce();
157  EXPECT_EQ(frame_collector_.frames.size(), 1);
158 
159  // wipe the frame queue.
160  frame_collector_.frames.clear();
161 
162 
163  // finally, check if frames with a dlc > 8 are discarded.
164  msg.dlc = 10;
165  publisher_.publish(msg);
166  ros::WallDuration(1.0).sleep();
167  ros::spinOnce();
168  EXPECT_EQ(frame_collector_.frames.size(), 0);
169 }
170 
171 int main(int argc, char **argv)
172 {
173  ros::init(argc, argv, "test_to_topic");
174  ros::NodeHandle nh;
175  ros::WallDuration(1.0).sleep();
176  testing::InitGoogleTest(&argc, argv);
177  return RUN_ALL_TESTS();
178 }
void publish(const boost::shared_ptr< M > &message) const
f
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::list< can::Frame > frames
void frameCallback(const can::Frame &f)
bool sleep() const
TEST(TopicToSocketCANTest, checkCorrectData)
void convertSocketCANToMessage(const can::Frame &f, can_msgs::Frame &m)
static Time now()
ROSCPP_DECL void spinOnce()
int main(int argc, char **argv)


socketcan_bridge
Author(s): Ivor Wanders
autogenerated on Fri May 14 2021 02:59:47