Skip to content

Commit 2d51b1b

Browse files
tmoritmori
committed
add cmd magnet
1 parent 82170e3 commit 2d51b1b

File tree

5 files changed

+320
-1
lines changed

5 files changed

+320
-1
lines changed

‎config/ros_msgs.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ hako_msgs/DroneStatus
55
hako_msgs/ShareObjectOwnerResponse
66
hako_msgs/ShareObjectOwner
77
hako_msgs/HakoDroneCmdMove
8+
hako_msgs/HakoCmdMagnetHolder
89
hako_msgs/DisturbanceWind
910
hako_msgs/HakoCameraData
1011
hako_msgs/SimpleVarray

‎pdu/csharp/hako_msgs/HakoCmdMagnetHolder.cs‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ namespace hakoniwa.pdu.msgs.hako_msgs
99
public class HakoCmdMagnetHolder
1010
{
1111
protected internal readonly IPdu _pdu;
12+
public IPdu GetPdu() { return _pdu; }
1213

1314
public HakoCmdMagnetHolder(IPdu pdu)
1415
{
@@ -28,7 +29,7 @@ public HakoCmdHeader header
2829
set
2930
{
3031
_header = value;
31-
_pdu.SetData("header", value._pdu);
32+
_pdu.SetData("header", value.GetPdu());
3233
}
3334
}
3435
public bool magnet_on

‎pdu/python/hako_msgs/pdu_conv_HakoCmdMagnetHolder.py‎

Lines changed: 155 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,155 @@
1+
2+
import struct
3+
from .pdu_pytype_HakoCmdMagnetHolder import HakoCmdMagnetHolder
4+
from ..pdu_utils import *
5+
from .. import binary_io
6+
7+
# dependencies for the generated Python class
8+
from ..hako_msgs.pdu_conv_HakoCmdHeader import *
9+
10+
11+
12+
def pdu_to_py_HakoCmdMagnetHolder(binary_data: bytearray) -> HakoCmdMagnetHolder:
13+
py_obj = HakoCmdMagnetHolder()
14+
meta_parser = binary_io.PduMetaDataParser()
15+
meta = meta_parser.load_pdu_meta(binary_data)
16+
if meta is None:
17+
raise ValueError("Invalid PDU binary data: MetaData not found or corrupted")
18+
binary_read_recursive_HakoCmdMagnetHolder(meta, binary_data, py_obj, binary_io.PduMetaData.PDU_META_DATA_SIZE)
19+
return py_obj
20+
21+
22+
def binary_read_recursive_HakoCmdMagnetHolder(meta: binary_io.PduMetaData, binary_data: bytearray, py_obj: HakoCmdMagnetHolder, base_off: int):
23+
# array_type: single
24+
# data_type: struct
25+
# member_name: header
26+
# type_name: HakoCmdHeader
27+
# offset: 0 size: 12
28+
# array_len: 1
29+
30+
tmp_py_obj = HakoCmdHeader()
31+
binary_read_recursive_HakoCmdHeader(meta, binary_data, tmp_py_obj, base_off + 0)
32+
py_obj.header = tmp_py_obj
33+
34+
# array_type: single
35+
# data_type: primitive
36+
# member_name: magnet_on
37+
# type_name: bool
38+
# offset: 12 size: 4
39+
# array_len: 1
40+
41+
42+
bin = binary_io.readBinary(binary_data, base_off + 12, 4)
43+
py_obj.magnet_on = binary_io.binTovalue("bool", bin)
44+
45+
return py_obj
46+
47+
48+
def py_to_pdu_HakoCmdMagnetHolder(py_obj: HakoCmdMagnetHolder) -> bytearray:
49+
binary_data = bytearray()
50+
base_allocator = DynamicAllocator(False)
51+
bw_container = BinaryWriterContainer(binary_io.PduMetaData())
52+
binary_write_recursive_HakoCmdMagnetHolder(0, bw_container, base_allocator, py_obj)
53+
54+
# メタデータの設定
55+
total_size = base_allocator.size() + bw_container.heap_allocator.size() + binary_io.PduMetaData.PDU_META_DATA_SIZE
56+
bw_container.meta.total_size = total_size
57+
bw_container.meta.heap_off = binary_io.PduMetaData.PDU_META_DATA_SIZE + base_allocator.size()
58+
59+
# binary_data のサイズを total_size に調整
60+
if len(binary_data) < total_size:
61+
binary_data.extend(bytearray(total_size - len(binary_data)))
62+
elif len(binary_data) > total_size:
63+
del binary_data[total_size:]
64+
65+
# メタデータをバッファにコピー
66+
binary_io.writeBinary(binary_data, 0, bw_container.meta.to_bytes())
67+
68+
# 基本データをバッファにコピー
69+
binary_io.writeBinary(binary_data, bw_container.meta.base_off, base_allocator.to_array())
70+
71+
# ヒープデータをバッファにコピー
72+
binary_io.writeBinary(binary_data, bw_container.meta.heap_off, bw_container.heap_allocator.to_array())
73+
74+
return binary_data
75+
76+
def binary_write_recursive_HakoCmdMagnetHolder(parent_off: int, bw_container: BinaryWriterContainer, allocator, py_obj: HakoCmdMagnetHolder):
77+
# array_type: single
78+
# data_type: struct
79+
# member_name: header
80+
# type_name: HakoCmdHeader
81+
# offset: 0 size: 12
82+
# array_len: 1
83+
type = "HakoCmdHeader"
84+
off = 0
85+
86+
binary_write_recursive_HakoCmdHeader(parent_off + off, bw_container, allocator, py_obj.header)
87+
88+
# array_type: single
89+
# data_type: primitive
90+
# member_name: magnet_on
91+
# type_name: bool
92+
# offset: 12 size: 4
93+
# array_len: 1
94+
type = "bool"
95+
off = 12
96+
97+
98+
bin = binary_io.typeTobin(type, py_obj.magnet_on)
99+
bin = get_binary(type, bin, 4)
100+
allocator.add(bin, expected_offset=parent_off + off)
101+
102+
103+
if __name__ == "__main__":
104+
import sys
105+
import json
106+
107+
def print_usage():
108+
print(f"Usage: python -m pdu.python.pdu_conv_HakoCmdMagnetHolder <read|write> [args...]")
109+
print(f" read <input_binary_file> <output_json_file>")
110+
print(f" write <input_json_file> <output_binary_file>")
111+
112+
if len(sys.argv) < 2:
113+
print_usage()
114+
sys.exit(1)
115+
116+
command = sys.argv[1]
117+
118+
if command == "read":
119+
if len(sys.argv) != 4:
120+
print_usage()
121+
sys.exit(1)
122+
123+
binary_filepath = sys.argv[2]
124+
output_json_filepath = sys.argv[3]
125+
126+
with open(binary_filepath, "rb") as f:
127+
binary_data = bytearray(f.read())
128+
129+
py_obj = pdu_to_py_HakoCmdMagnetHolder(binary_data)
130+
131+
with open(output_json_filepath, "w") as f:
132+
f.write(py_obj.to_json())
133+
134+
elif command == "write":
135+
if len(sys.argv) != 4:
136+
print_usage()
137+
sys.exit(1)
138+
139+
input_json_filepath = sys.argv[2]
140+
output_binary_filepath = sys.argv[3]
141+
142+
with open(input_json_filepath, "r") as f:
143+
json_str = f.read()
144+
145+
py_obj = HakoCmdMagnetHolder.from_json(json_str)
146+
147+
binary_data = py_to_pdu_HakoCmdMagnetHolder(py_obj)
148+
149+
with open(output_binary_filepath, "wb") as f:
150+
f.write(binary_data)
151+
152+
else:
153+
print(f"Unknown command: {command}")
154+
print_usage()
155+
sys.exit(1)

‎pdu/python/hako_msgs/pdu_pytype_HakoCmdMagnetHolder.py‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
from typing import List
2+
3+
# dependencies for the generated Python class
4+
from ..hako_msgs.pdu_pytype_HakoCmdHeader import HakoCmdHeader
5+
6+
7+
# class definition for the ROS message
8+
class HakoCmdMagnetHolder:
9+
"""
10+
ROS message: hako_msgs/HakoCmdMagnetHolder
11+
Generated by hakoniwa-ros2pdu.
12+
"""
13+
14+
header: HakoCmdHeader
15+
magnet_on: bool
16+
17+
def __init__(self):
18+
self.header = HakoCmdHeader()
19+
self.magnet_on = False
20+
21+
def __str__(self):
22+
return f"HakoCmdMagnetHolder(" + ", ".join([
23+
f"header={self.header}"
24+
f"magnet_on={self.magnet_on}"
25+
]) + ")"
26+
27+
def __repr__(self):
28+
return self.__str__()
29+
30+
def to_dict(self):
31+
"""
32+
Convert the object to a dictionary.
33+
"""
34+
d = {}
35+
# handle field 'header'
36+
field_val = self.header
37+
if isinstance(field_val, bytearray):
38+
d['header'] = list(field_val)
39+
elif hasattr(field_val, 'to_dict'):
40+
d['header'] = field_val.to_dict()
41+
elif isinstance(field_val, list):
42+
d['header'] = [item.to_dict() if hasattr(item, 'to_dict') else item for item in field_val]
43+
else:
44+
d['header'] = field_val
45+
# handle field 'magnet_on'
46+
field_val = self.magnet_on
47+
if isinstance(field_val, bytearray):
48+
d['magnet_on'] = list(field_val)
49+
elif hasattr(field_val, 'to_dict'):
50+
d['magnet_on'] = field_val.to_dict()
51+
elif isinstance(field_val, list):
52+
d['magnet_on'] = [item.to_dict() if hasattr(item, 'to_dict') else item for item in field_val]
53+
else:
54+
d['magnet_on'] = field_val
55+
return d
56+
57+
@classmethod
58+
def from_dict(cls, d: dict):
59+
"""
60+
Create an object from a dictionary.
61+
"""
62+
obj = cls()
63+
# handle field 'header'
64+
if 'header' in d:
65+
field_type = cls.__annotations__.get('header')
66+
value = d['header']
67+
68+
if field_type is bytearray:
69+
obj.header = bytearray(value)
70+
elif hasattr(field_type, '__origin__') and field_type.__origin__ is list:
71+
list_item_type = field_type.__args__[0]
72+
if hasattr(list_item_type, 'from_dict'):
73+
obj.header = [list_item_type.from_dict(item) for item in value]
74+
else:
75+
obj.header = value
76+
elif hasattr(field_type, 'from_dict'):
77+
obj.header = field_type.from_dict(value)
78+
else:
79+
obj.header = value
80+
# handle field 'magnet_on'
81+
if 'magnet_on' in d:
82+
field_type = cls.__annotations__.get('magnet_on')
83+
value = d['magnet_on']
84+
85+
if field_type is bytearray:
86+
obj.magnet_on = bytearray(value)
87+
elif hasattr(field_type, '__origin__') and field_type.__origin__ is list:
88+
list_item_type = field_type.__args__[0]
89+
if hasattr(list_item_type, 'from_dict'):
90+
obj.magnet_on = [list_item_type.from_dict(item) for item in value]
91+
else:
92+
obj.magnet_on = value
93+
elif hasattr(field_type, 'from_dict'):
94+
obj.magnet_on = field_type.from_dict(value)
95+
else:
96+
obj.magnet_on = value
97+
return obj
98+
99+
def to_json(self, indent=2):
100+
"""
101+
Convert the object to a JSON string.
102+
"""
103+
import json
104+
return json.dumps(self.to_dict(), indent=indent)
105+
106+
@classmethod
107+
def from_json(cls, json_str: str):
108+
"""
109+
Create an object from a JSON string.
110+
"""
111+
import json
112+
return cls.from_dict(json.loads(json_str))

‎pdu/types/hako_msgs/pdu_cpptype_conv_HakoCmdMagnetHolder.hpp‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -100,4 +100,54 @@ static inline Hako_HakoCmdMagnetHolder* hako_create_empty_pdu_HakoCmdMagnetHolde
100100
}
101101
return (Hako_HakoCmdMagnetHolder*)base_ptr;
102102
}
103+
namespace hako::pdu::msgs::hako_msgs
104+
{
105+
class HakoCmdMagnetHolder
106+
{
107+
public:
108+
HakoCmdMagnetHolder() = default;
109+
~HakoCmdMagnetHolder() = default;
110+
111+
bool pdu2cpp(char* top_ptr, HakoCpp_HakoCmdMagnetHolder& cppData)
112+
{
113+
char* base_ptr = (char*)hako_get_base_ptr_pdu((void*)top_ptr);
114+
if (base_ptr == nullptr) {
115+
std::cerr << "[ConvertorError][" << "HakoCmdMagnetHolder" << "] hako_get_base_ptr_pdu returned null" << std::endl;
116+
return false;
117+
}
118+
int ret = hako_convert_pdu2cpp_HakoCmdMagnetHolder(*(Hako_HakoCmdMagnetHolder*)base_ptr, cppData);
119+
if (ret != 0) {
120+
std::cerr << "[ConvertorError][" << "HakoCmdMagnetHolder" << "] hako_convert_pdu2cpp returned " << ret << std::endl;
121+
return false;
122+
}
123+
return true;
124+
}
125+
126+
int cpp2pdu(HakoCpp_HakoCmdMagnetHolder& cppData, char* pdu_buffer, int buffer_len)
127+
{
128+
char* base_ptr = nullptr;
129+
int pdu_size = hako_convert_cpp2pdu_HakoCmdMagnetHolder(cppData, (Hako_HakoCmdMagnetHolder**)&base_ptr);
130+
if (pdu_size < 0) {
131+
std::cerr << "[ConvertorError][" << "HakoCmdMagnetHolder" << "] hako_convert_cpp2pdu returned error code: " << pdu_size << std::endl;
132+
return -1;
133+
}
134+
if (pdu_size > buffer_len) {
135+
std::cerr << "[ConvertorError][" << "HakoCmdMagnetHolder" << "] buffer too small. pdu_size=" << pdu_size << " buffer_len=" << buffer_len << std::endl;
136+
return -1;
137+
}
138+
void* top_ptr = hako_get_top_ptr_pdu((void*)base_ptr);
139+
if (top_ptr == nullptr) {
140+
std::cerr << "[ConvertorError][" << "HakoCmdMagnetHolder" << "] hako_get_top_ptr_pdu returned null" << std::endl;
141+
return false;
142+
}
143+
memcpy(pdu_buffer, top_ptr, pdu_size);
144+
hako_destroy_pdu((void*)base_ptr);
145+
return pdu_size;
146+
}
147+
148+
private:
149+
};
150+
}
151+
152+
103153
#endif /* _PDU_CPPTYPE_CONV_HAKO_hako_msgs_HakoCmdMagnetHolder_HPP_ */

0 commit comments

Comments
 (0)