Open igricart opened 2 years ago
@igricart To understand your use case, please confirm the followings.
Your PLC node id = 21 (I am assuming as 0x1A1 - 0x180 = 0x21 ).
To map a Receive PDO in the master, we use the method add_receive_pdo_mapping
as follows.
device_->add_receive_pdo_mapping(MASTER_RPDO_BASE_ADDRESS + node_id,
entry_index, entry_subindex, starting offset_for_the_payload);
or
device_->add_receive_pdo_mapping(MASTER_RPDO_BASE_ADDRESS + node_id,
entry_name_generated_by_eds_parser_as_string,
starting offset_for_the_payload);
The PLC must send the corresponding TPDO. For example, if you have mapped the RPDO in master for the base address 0x180 (This is for RPDO1), then the PLC must send a data packet that will look like 180 # xx xx xx xx xx xx xx xx
If the PLC does not send the data packet, then you need to map the TPDO in the PLC by using the method map_tpdo_in_device()
Also, to get a successful RPDO mapping, the entry_index and subindex must be valid. Are you sure that the entry plc2pc_timestamp_pc_1/plc2pc_timestamp_pc_1
exists in the EDS file? can you share the object_entry dump by the EDS parser and the EDS file ?
As you said, the node id is 33, which turns 0x21. Both the PC and PLC have TPDO and RPDO set, which I could check through the messages from candump can0
:
can0 721 [1] 05
can0 1A1 [4] 07 2C DA B5
can0 2A1 [8] 7B 02 00 00 00 00 00 00
can0 221 [4] 07 2C DA B5
can0 321 [8] 7C 02 00 00 00 00 00 00
can0 1A1 [4] 67 79 DA B5
can0 2A1 [8] 7C 02 00 00 00 00 00 00
can0 221 [4] 67 79 DA B5
can0 321 [8] 7D 02 00 00 00 00 00 00
can0 1A1 [4] F2 C7 DA B5
can0 2A1 [8] 7D 02 00 00 00 00 00 00
can0 221 [4] F2 C7 DA B5
can0 321 [8] 7E 02 00 00 00 00 00 00
can0 1A1 [4] B1 15 DB B5
can0 2A1 [8] 7E 02 00 00 00 00 00 00
can0 1A1 [4] 67 63 DB B5
can0 221 [4] 67 63 DB B5
can0 321 [8] 7F 02 00 00 00 00 00 00
can0 1A1 [4] 04 B2 DB B5
can0 2A1 [8] 7F 02 00 00 00 00 00 00
can0 221 [4] 04 B2 DB B5
can0 321 [8] 80 02 00 00 00 00 00 00
can0 1A1 [4] 78 00 DC B5
can0 2A1 [8] 80 02 00 00 00 00 00 00
The output from the function print_dictionary()
(right after loading the eds file -renamed to txt so I could attach to github - Test_TimeStamp_2.eds) is:
0x1000/0 const device_type empty
0x1001/0 ro error_register empty
0x1003/0 rw predefined_error_field/number_of_errors empty
0x1003/1 ro predefined_error_field/standard_error_field empty
0x1003/2 ro predefined_error_field/standard_error_field_2 empty
0x1003/3 ro predefined_error_field/standard_error_field_3 empty
0x1005/0 rw sync_cob_id empty
0x1006/0 rw communication_cycle_period empty
0x100c/0 rw guard_time empty
0x100d/0 rw life_time_factor empty
0x1014/0 rw cob_id_emcy empty
0x1016/0 ro consumer_heartbeat_time/number_of_entries empty
0x1016/1 rw consumer_heartbeat_time/consumer_heartbeat_time empty
0x1017/0 rw producer_heartbeat_time 400
0x1018/0 ro identity_object/number_of_entries empty
0x1018/1 ro identity_object/vendor_id empty
0x1018/2 ro identity_object/product_code empty
0x1018/3 ro identity_object/revision_number empty
0x1200/0 ro serversdoparameter/number_of_entries empty
0x1200/1 ro serversdoparameter/cob_id_client_>server empty
0x1200/2 ro serversdoparameter/cob_id_server_>client empty
0x1400/0 ro receive_pdo_communication_parameter_1/number_of_entries empty
0x1400/1 rw receive_pdo_communication_parameter_1/cob_id empty
0x1400/2 rw receive_pdo_communication_parameter_1/transmission_type empty
0x1401/0 ro receive_pdo_communication_parameter_2/number_of_entries empty
0x1401/1 rw receive_pdo_communication_parameter_2/cob_id empty
0x1401/2 rw receive_pdo_communication_parameter_2/transmission_type empty
0x1402/0 ro receive_pdo_communication_parameter_3/number_of_entries empty
0x1402/1 rw receive_pdo_communication_parameter_3/cob_id empty
0x1402/2 rw receive_pdo_communication_parameter_3/transmission_type empty
0x1600/0 const receive_pdo_mapping_parameter_1/number_of_entries empty
0x1600/1 const receive_pdo_mapping_parameter_1/mapping_entry empty
0x1601/0 const receive_pdo_mapping_parameter_2/number_of_entries empty
0x1601/1 const receive_pdo_mapping_parameter_2/mapping_entry empty
0x1602/0 const receive_pdo_mapping_parameter_3/number_of_entries empty
0x1602/1 const receive_pdo_mapping_parameter_3/mapping_entry empty
0x1800/0 ro transmit_pdo_communication_parameter_1/number_of_entries empty
0x1800/1 rw transmit_pdo_communication_parameter_1/cob_id empty
0x1800/2 rw transmit_pdo_communication_parameter_1/transmission_type empty
0x1800/3 rw transmit_pdo_communication_parameter_1/inhibit_time empty
0x1800/4 rw transmit_pdo_communication_parameter_1/reserved empty
0x1800/5 rw transmit_pdo_communication_parameter_1/event_timer empty
0x1801/0 ro transmit_pdo_communication_parameter_2/number_of_entries empty
0x1801/1 rw transmit_pdo_communication_parameter_2/cob_id empty
0x1801/2 rw transmit_pdo_communication_parameter_2/transmission_type empty
0x1801/3 rw transmit_pdo_communication_parameter_2/inhibit_time empty
0x1801/4 rw transmit_pdo_communication_parameter_2/reserved empty
0x1801/5 rw transmit_pdo_communication_parameter_2/event_timer empty
0x1802/0 ro transmit_pdo_communication_parameter_3/number_of_entries empty
0x1802/1 rw transmit_pdo_communication_parameter_3/cob_id empty
0x1802/2 rw transmit_pdo_communication_parameter_3/transmission_type empty
0x1802/3 rw transmit_pdo_communication_parameter_3/inhibit_time empty
0x1802/4 rw transmit_pdo_communication_parameter_3/reserved empty
0x1802/5 rw transmit_pdo_communication_parameter_3/event_timer empty
0x1a00/0 const transmit_pdo_mapping_parameter_1/number_of_entries empty
0x1a00/1 const transmit_pdo_mapping_parameter_1/mapping_entry empty
0x1a01/0 const transmit_pdo_mapping_parameter_2/number_of_entries empty
0x1a01/1 const transmit_pdo_mapping_parameter_2/mapping_entry empty
0x1a02/0 const transmit_pdo_mapping_parameter_3/number_of_entries empty
0x1a02/1 const transmit_pdo_mapping_parameter_3/mapping_entry empty
0x3000/0 const pc2plc_timestamp_plc_1/number_of_entries empty
0x3000/1 wo pc2plc_timestamp_plc_1/pc2plc_timestamp_plc_1 empty
0x3001/0 const pc2plc_timestamp_pc_1/number_of_entries empty
0x3001/1 wo pc2plc_timestamp_pc_1/pc2plc_timestamp_pc_1 empty
0x3002/0 const test_output_1/number_of_entries empty
0x3002/1 wo test_output_1/test_output_1 empty
0x3800/0 const plc2pc_timestamp_plc_1/number_of_entries empty
0x3800/1 ro plc2pc_timestamp_plc_1/plc2pc_timestamp_plc_1 empty
0x3801/0 const plc2pc_timestamp_pc_1/number_of_entries empty
0x3801/1 ro plc2pc_timestamp_pc_1/plc2pc_timestamp_pc_1 empty
0x3802/0 const test_input_1/number_of_entries empty
0x3802/1 ro test_input_1/test_input_1 empty
@igricart What I can see from the EDS file that the datatype for plc2pc_timestamp_pc_1/plc2pc_timestamp_pc_1
is 0x001B i.e unsigned int64. I have never tried to map uint64 entry. On the other hand the working PDO mapping entry plc2pc_timestamp_plc_1/plc2pc_timestamp_plc_1
is of datatype 0x0007 i.e unsigned int32. The datatypes I have tried and got working are
INTIGER8 = 0x0002,
INTIGER16 = 0x0003,
INTIGER32 = 0x0004,
UNSIGNED_INTIGER8 = 0x0005,
UNSIGNED_INTIGER16 = 0x0006,
UNSIGNED_INTIGER32 = 0x0007,
REAL32 = 0x0008,
STRING = 0x0009,
Can you try to map another 32bit entry to check wether it is working or not? Also please share the exception message.
The exception is: double free or corruption (fasttop)
Stack trace (most recent call last):
#29 Object "", at 0xffffffffffffffff, in
#28 Object "/home/imr/Documents/ps/tmp/canbus/devel/.private/can_communication/lib/can_communication/can_communication_node", at 0x5653271d7ad9, in _start
#27 Source "../csu/libc-start.c", line 310, in __libc_start_main [0x7f672021ac86]
#26 Source "/home/imr/Documents/ps/tmp/canbus/src/can_communication/src/node.cpp", line 6, in main [0x5653271d7dcd]
3: int main(int argc, char** argv) {
4: ros::init(argc, argv, "master_can");
5: ros::NodeHandle nh("");
> 6: can_communication::MasterCAN master(nh);
7:
8: master.start();
9: master.reset_node();
#25 Source "/home/imr/Documents/ps/tmp/canbus/src/can_communication/src/master_can.cpp", line 7, in ~MasterCAN [0x7f6720c4ac78]
5: MasterCAN::MasterCAN(ros::NodeHandle& nh) : MasterCANInterface(nh) {}
6:
> 7: MasterCAN::~MasterCAN() { std::cout << "Killing Master Node" << std::endl; }
8:
9: bool MasterCAN::start() {
10: ROS_INFO("Start Core");
#24 Source "/home/imr/Documents/ps/tmp/canbus/src/can_communication/src/master_can_interface.cpp", line 15, in ~MasterCANInterface [0x7f6720c52d3b]
12: pnh_.param<std::string>("eds_file", eds_file_, "1M");
13: };
14:
> 15: MasterCANInterface::~MasterCANInterface(){};
16:
17: } // namespace can_communication
#23 Source "/usr/include/c++/9/bits/unique_ptr.h", line 292, in ~unique_ptr [0x7f6720c4e7dd]
289: "unique_ptr's deleter must be invocable with a pointer");
290: auto& __ptr = _M_t._M_ptr();
291: if (__ptr != nullptr)
> 292: get_deleter()(std::move(__ptr));
293: __ptr = pointer();
294: }
#22 Source "/usr/include/c++/9/bits/unique_ptr.h", line 81, in operator() [0x7f6720c4f3c1]
78: "can't delete pointer to incomplete type");
79: static_assert(sizeof(_Tp)>0,
80: "can't delete pointer to incomplete type");
> 81: delete __ptr;
82: }
83: };
#21 Source "/home/imr/Documents/ps/tmp/canbus/src/kacanopen/src/master/device.cpp", line 56, in ~Device [0x7f671dbbe34b]
53: terminating_(false) {}
54:
55: Device::~Device() {
> 56: for (auto& cob_id : cob_ids_) m_core.pdo.remove_pdo_received_callback(cob_id);
57: stop_request_heartbeat();
58: }
#20 Source "/home/imr/Documents/ps/tmp/canbus/src/kacanopen/src/core/pdo.cpp", line 109, in remove_pdo_received_callback [0x7f671def9dfd]
106: for (auto i = m_receive_callbacks.begin(); i != m_receive_callbacks.end();
107: i++)
108: if ((*i).cob_id == cob_id) {
> 109: m_receive_callbacks.erase(i);
110: break;
111: }
112: }
#19 Source "/usr/include/c++/9/bits/stl_vector.h", line 1428, in erase [0x7f671defa618]
1425: iterator
1426: #if __cplusplus >= 201103L
1427: erase(const_iterator __position)
>1428: { return _M_erase(begin() + (__position - cbegin())); }
1429: #else
1430: erase(iterator __position)
1431: { return _M_erase(__position); }
#18 Source "/usr/include/c++/9/bits/vector.tcc", line 177, in _M_erase [0x7f671defaf5b]
174: if (__position + 1 != end())
175: _GLIBCXX_MOVE3(__position + 1, end(), __position);
176: --this->_M_impl._M_finish;
> 177: _Alloc_traits::destroy(this->_M_impl, this->_M_impl._M_finish);
178: _GLIBCXX_ASAN_ANNOTATE_SHRINK(1);
179: return __position;
180: }
#17 Source "/usr/include/c++/9/bits/alloc_traits.h", line 496, in destroy<kaco::PDO::PDOReceivedCallback> [0x7f671defb803]
493: static void
494: destroy(allocator_type& __a, _Up* __p)
495: noexcept(noexcept(__a.destroy(__p)))
> 496: { __a.destroy(__p); }
497:
498: /**
499: * @brief The maximum supported allocation size
#16 Source "/usr/include/c++/9/ext/new_allocator.h", line 152, in destroy<kaco::PDO::PDOReceivedCallback> [0x7f671defbcaf]
149: void
150: destroy(_Up* __p)
151: noexcept(std::is_nothrow_destructible<_Up>::value)
> 152: { __p->~_Up(); }
153: #else
154: // _GLIBCXX_RESOLVE_LIB_DEFECTS
155: // 402. wrong new expression in [some_] allocator::construct
#15 Source "/home/imr/Documents/ps/tmp/canbus/src/kacanopen/include/kacanopen/core/pdo.h", line 57, in ~PDOReceivedCallback [0x7f671deeb785]
54: /// together with it's COB-ID
55: /// Important: Never call add_pdo_received_callback or
56: /// process_incoming_message from within (-> deadlock)!
> 57: struct PDOReceivedCallback {
58: /// Type of the callback
59: using Callback = std::function<void(std::vector<uint8_t>)>;
#14 Source "/usr/include/c++/9/bits/std_function.h", line 369, in ~function [0x7f671deeb765]
366: * Polymorphic function wrapper.
367: */
368: template<typename _Res, typename... _ArgTypes>
> 369: class function<_Res(_ArgTypes...)>
370: : public _Maybe_unary_or_binary_function<_Res, _ArgTypes...>,
371: private _Function_base
372: {
#13 Source "/usr/include/c++/9/bits/std_function.h", line 260, in ~_Function_base [0x7f6720c4da98]
257: ~_Function_base()
258: {
259: if (_M_manager)
> 260: _M_manager(_M_functor, _M_functor, __destroy_functor);
261: }
262:
263: bool _M_empty() const { return !_M_manager; }
#12 Source "/usr/include/c++/9/bits/std_function.h", line 215, in _M_manager [0x7f671dbcb75f]
212: break;
213:
214: case __destroy_functor:
> 215: _M_destroy(__dest, _Local_storage());
216: break;
217: }
218: return false;
#11 Source "/usr/include/c++/9/bits/std_function.h", line 191, in _M_destroy [0x7f671dbcfdb3]
188: static void
189: _M_destroy(_Any_data& __victim, false_type)
190: {
> 191: delete __victim._M_access<_Functor*>();
192: }
193:
194: public:
#10 Source "/usr/include/c++/9/functional", line 386, in ~_Bind [0x7f671dbc6d85]
383: struct _Bind;
384:
385: template<typename _Functor, typename... _Bound_args>
> 386: class _Bind<_Functor(_Bound_args...)>
387: : public _Weak_result_type<_Functor>
388: {
389: typedef typename _Build_index_tuple<sizeof...(_Bound_args)>::__type
#9 Source "/usr/include/c++/9/tuple", line 523, in ~tuple [0x7f671dbc6d65]
521: /// Primary class template, tuple
522: template<typename... _Elements>
> 523: class tuple : public _Tuple_impl<0, _Elements...>
524: {
525: typedef _Tuple_impl<0, _Elements...> _Inherited;
#8 Source "/usr/include/c++/9/tuple", line 185, in ~_Tuple_impl [0x7f671dbc6d49]
182: * (which contains the @c Tail).
183: */
184: template<std::size_t _Idx, typename _Head, typename... _Tail>
> 185: struct _Tuple_impl<_Idx, _Head, _Tail...>
186: : public _Tuple_impl<_Idx + 1, _Tail...>,
187: private _Head_base<_Idx, _Head>
188: {
#7 Source "/usr/include/c++/9/tuple", line 185, in ~_Tuple_impl [0x7f671dbc6d2d]
182: * (which contains the @c Tail).
183: */
184: template<std::size_t _Idx, typename _Head, typename... _Tail>
> 185: struct _Tuple_impl<_Idx, _Head, _Tail...>
186: : public _Tuple_impl<_Idx + 1, _Tail...>,
187: private _Head_base<_Idx, _Head>
188: {
#6 Source "/usr/include/c++/9/tuple", line 120, in ~_Head_base [0x7f671dbc6d11]
117: };
118:
119: template<std::size_t _Idx, typename _Head>
> 120: struct _Head_base<_Idx, _Head, false>
121: {
122: constexpr _Head_base()
123: : _M_head_impl() { }
#5 Source "/home/imr/Documents/ps/tmp/canbus/src/kacanopen/include/kacanopen/master/receive_pdo_mapping.h", line 42, in ~ReceivePDOMapping [0x7f671dbc6cf5]
39: /// to one dictionary entry. There may be multiple
40: /// ReceivePDOMapping instances for one PDO.
41: /// \todo Add index/subindex overload?
> 42: struct ReceivePDOMapping {
43: /// COB-ID of the PDO
44: uint16_t cob_id;
#4 | Source "/build/glibc-uZu3wS/glibc-2.27/malloc/malloc.c", line 3134, in _int_free
Source "/build/glibc-uZu3wS/glibc-2.27/malloc/malloc.c", line 4260, in __libc_free [0x7f6720290fd3]
#3 Source "/build/glibc-uZu3wS/glibc-2.27/malloc/malloc.c", line 5342, in malloc_printerr [0x7f67202898b9]
#2 Source "../sysdeps/posix/libc_fatal.c", line 181, in __stack_chk_fail [0x7f6720282836]
#1 Source "/build/glibc-uZu3wS/glibc-2.27/stdlib/abort.c", line 79, in abort [0x7f67202397f0]
#0 Source "../sysdeps/unix/sysv/linux/raise.c", line 51, in raise [0x7f6720237e87]
Aborted (Signal sent by tkill() 7948 1000)
Currently, I have a device connected to a PLC, which communicates well as long as I only make one
add_receive_pdo_mapping()
call. If I try calling that function twice for different entries, I have an exception happening during the destructor call.I have a function calling
When I press
Ctrl + C
, the following error happens:free(): double free detected in tcache 2
The last 5 calls in stack trace are
It seems that the
ReceivePDOMapping
destructor tries to run twice in an object, which has already been destroyed. It could be related to this part of thedevice.cpp
code:Any hints on what could I be doing wrong? I saw that there is a file
simple_pdo_rw_backward_compatible.cpp
which uses the methodmap_tpdo_in_device()
. Is that necessary when handling more than one pdo?