Skip to content

Commit 00fb72e

Browse files
HiroyukiKanaiHiroyukiKanai
committed
ToF msg(0x00F)のtopicを分離
1 parent de7fee9 commit 00fb72e

File tree

2 files changed

+74
-79
lines changed

2 files changed

+74
-79
lines changed

‎socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@ class SocketCANToTopic
4545
void setup(ros::NodeHandle nh);
4646

4747
private:
48-
ros::Publisher can_topic_;
48+
ros::Publisher can_topic_, can_topic_tof_;
4949
can::DriverInterfaceSharedPtr driver_;
5050

5151
can::FrameListenerConstSharedPtr frame_listener_;

‎socketcan_bridge/src/socketcan_to_topic.cpp

+73-78
Original file line numberDiff line numberDiff line change
@@ -25,109 +25,104 @@
2525
* POSSIBILITY OF SUCH DAMAGE.
2626
*/
2727

28+
#include <can_msgs/Frame.h>
2829
#include <socketcan_bridge/socketcan_to_topic.h>
2930
#include <socketcan_interface/string.h>
30-
#include <can_msgs/Frame.h>
31+
3132
#include <string>
3233

3334
namespace can
3435
{
35-
template<> can::FrameFilterSharedPtr tofilter(const XmlRpc::XmlRpcValue &ct)
36+
template <>
37+
can::FrameFilterSharedPtr tofilter(const XmlRpc::XmlRpcValue & ct)
3638
{
3739
XmlRpc::XmlRpcValue t(ct);
3840
try // try to read as integer
3941
{
4042
uint32_t id = static_cast<int>(t);
4143
return tofilter(id);
42-
}
43-
catch(...) // else read as string
44+
} catch (...) // else read as string
4445
{
45-
return tofilter(static_cast<std::string>(t));
46+
return tofilter(static_cast<std::string>(t));
4647
}
4748
}
4849
} // namespace can
4950

5051
namespace socketcan_bridge
5152
{
52-
SocketCANToTopic::SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
53-
can::DriverInterfaceSharedPtr driver)
54-
{
55-
can_topic_ = nh->advertise<can_msgs::Frame>("received_messages",
56-
nh_param->param("received_messages_queue_size", 10));
57-
driver_ = driver;
58-
};
59-
60-
void SocketCANToTopic::setup()
61-
{
62-
// register handler for frames and state changes.
63-
frame_listener_ = driver_->createMsgListenerM(this, &SocketCANToTopic::frameCallback);
64-
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
65-
};
66-
67-
void SocketCANToTopic::setup(const can::FilteredFrameListener::FilterVector &filters)
68-
{
69-
frame_listener_.reset(new can::FilteredFrameListener(driver_,
70-
std::bind(&SocketCANToTopic::frameCallback,
71-
this,
72-
std::placeholders::_1),
73-
filters));
74-
75-
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
76-
}
53+
SocketCANToTopic::SocketCANToTopic(
54+
ros::NodeHandle * nh, ros::NodeHandle * nh_param, can::DriverInterfaceSharedPtr driver)
55+
{
56+
can_topic_ = nh->advertise<can_msgs::Frame>(
57+
"received_messages", nh_param->param("received_messages_queue_size", 10));
58+
can_topic_tof_ = nh->advertise<can_msgs::Frame>(
59+
"received_tof_messages", nh_param->param("received_messages_queue_size", 10));
60+
driver_ = driver;
61+
};
7762

78-
void SocketCANToTopic::setup(XmlRpc::XmlRpcValue filters)
79-
{
80-
setup(can::tofilters(filters));
81-
}
82-
void SocketCANToTopic::setup(ros::NodeHandle nh)
83-
{
84-
XmlRpc::XmlRpcValue filters;
85-
if (nh.getParam("can_ids", filters)) return setup(filters);
86-
return setup();
87-
}
63+
void SocketCANToTopic::setup()
64+
{
65+
// register handler for frames and state changes.
66+
frame_listener_ = driver_->createMsgListenerM(this, &SocketCANToTopic::frameCallback);
67+
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
68+
};
8869

70+
void SocketCANToTopic::setup(const can::FilteredFrameListener::FilterVector & filters)
71+
{
72+
frame_listener_.reset(new can::FilteredFrameListener(
73+
driver_, std::bind(&SocketCANToTopic::frameCallback, this, std::placeholders::_1), filters));
8974

90-
void SocketCANToTopic::frameCallback(const can::Frame& f)
91-
{
92-
// ROS_DEBUG("Message came in: %s", can::tostring(f, true).c_str());
93-
if (!f.isValid())
94-
{
95-
ROS_ERROR("Invalid frame from SocketCAN: id: %#04x, length: %d, is_extended: %d, is_error: %d, is_rtr: %d",
96-
f.id, f.dlc, f.is_extended, f.is_error, f.is_rtr);
97-
return;
98-
}
99-
else
100-
{
101-
if (f.is_error)
102-
{
103-
// can::tostring cannot be used for dlc > 8 frames. It causes an crash
104-
// due to usage of boost::array for the data array. The should always work.
105-
ROS_WARN("Received frame is error: %s", can::tostring(f, true).c_str());
106-
}
107-
}
75+
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
76+
}
10877

109-
can_msgs::Frame msg;
110-
// converts the can::Frame (socketcan.h) to can_msgs::Frame (ROS msg)
111-
convertSocketCANToMessage(f, msg);
78+
void SocketCANToTopic::setup(XmlRpc::XmlRpcValue filters) { setup(can::tofilters(filters)); }
79+
void SocketCANToTopic::setup(ros::NodeHandle nh)
80+
{
81+
XmlRpc::XmlRpcValue filters;
82+
if (nh.getParam("can_ids", filters)) return setup(filters);
83+
return setup();
84+
}
11285

113-
msg.header.frame_id = ""; // empty frame is the de-facto standard for no frame.
114-
msg.header.stamp = ros::Time::now();
86+
void SocketCANToTopic::frameCallback(const can::Frame & f)
87+
{
88+
// ROS_DEBUG("Message came in: %s", can::tostring(f, true).c_str());
89+
if (!f.isValid()) {
90+
ROS_ERROR(
91+
"Invalid frame from SocketCAN: id: %#04x, length: %d, is_extended: %d, is_error: %d, is_rtr: "
92+
"%d",
93+
f.id, f.dlc, f.is_extended, f.is_error, f.is_rtr);
94+
return;
95+
} else {
96+
if (f.is_error) {
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+
}
115102

116-
can_topic_.publish(msg);
117-
};
103+
can_msgs::Frame msg;
104+
// converts the can::Frame (socketcan.h) to can_msgs::Frame (ROS msg)
105+
convertSocketCANToMessage(f, msg);
118106

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_ID_ROS_COMM_TOF_RANGE : 0x00F
111+
if (msg.id == 0x00F) {
112+
can_topic_tof_.publish(msg);
113+
} else {
114+
can_topic_.publish(msg);
115+
}
116+
};
119117

120-
void SocketCANToTopic::stateCallback(const can::State & s)
121-
{
122-
std::string err;
123-
driver_->translateError(s.internal_error, err);
124-
if (!s.internal_error)
125-
{
126-
ROS_DEBUG("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
127-
}
128-
else
129-
{
130-
ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
131-
}
132-
};
118+
void SocketCANToTopic::stateCallback(const can::State & s)
119+
{
120+
std::string err;
121+
driver_->translateError(s.internal_error, err);
122+
if (!s.internal_error) {
123+
ROS_DEBUG("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
124+
} else {
125+
ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
126+
}
127+
};
133128
}; // namespace socketcan_bridge

0 commit comments

Comments
 (0)