Skip to content

Commit

Permalink
updated for varray
Browse files Browse the repository at this point in the history
  • Loading branch information
@tmori
tmori committed Jun 14, 2024
1 parent 8ba1517 commit 8fbed6b
Show file tree
Hide file tree
Showing 8 changed files with 97 additions and 35 deletions.
30 changes: 15 additions & 15 deletions hakoniwa/config/custom.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter",
"channel_id": 0,
"pdu_size": 88,
"pdu_size": 112,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -23,7 +23,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReader",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduReaderConverter",
"channel_id": 1,
"pdu_size": 48,
"pdu_size": 72,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -34,7 +34,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 3,
"pdu_size": 56,
"pdu_size": 80,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -45,7 +45,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 5,
"pdu_size": 40,
"pdu_size": 64,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -56,7 +56,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 6,
"pdu_size": 56,
"pdu_size": 80,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -67,7 +67,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 7,
"pdu_size": 40,
"pdu_size": 64,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -78,7 +78,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 8,
"pdu_size": 52,
"pdu_size": 76,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -89,7 +89,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 9,
"pdu_size": 16,
"pdu_size": 40,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -100,7 +100,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 11,
"pdu_size": 20,
"pdu_size": 44,
"write_cycle": 1,
"method_type": "SHM"
}
Expand All @@ -113,7 +113,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 2,
"pdu_size": 280,
"pdu_size": 304,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -124,7 +124,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 4,
"pdu_size": 8,
"pdu_size": 32,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -135,7 +135,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 10,
"pdu_size": 8,
"pdu_size": 32,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -146,7 +146,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 12,
"pdu_size": 102668,
"pdu_size": 102716,
"write_cycle": 1,
"method_type": "SHM"
},
Expand All @@ -157,7 +157,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 13,
"pdu_size": 177376,
"pdu_size": 177400,
"write_cycle": 5,
"method_type": "SHM"
},
Expand All @@ -168,7 +168,7 @@
"class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriter",
"conv_class_name": "Hakoniwa.PluggableAsset.Communication.Pdu.Raw.RawPduWriterConverter",
"channel_id": 14,
"pdu_size": 48,
"pdu_size": 72,
"write_cycle": 5,
"method_type": "SHM"
}
Expand Down
2 changes: 1 addition & 1 deletion hakoniwa/config/sample_control/drone_config_0.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
"physicsEquation": "BodyFrame",
"collision_detection": true,
"manual_control": false,
"airFrictionCoefficient": [ 0.0001, 0.0 ],
"airFrictionCoefficient": [ 0.02, 0.0 ],
"inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
"mass_kg": 0.1,
"body_size": [ 0.1, 0.1, 0.01 ],
Expand Down
2 changes: 1 addition & 1 deletion hakoniwa/config/sample_control/drone_config_1.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
"physicsEquation": "BodyFrame",
"collision_detection": true,
"manual_control": false,
"airFrictionCoefficient": [ 0.0001, 0.0 ],
"airFrictionCoefficient": [ 0.02, 0.0 ],
"inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
"mass_kg": 0.1,
"body_size": [ 0.1, 0.1, 0.01 ],
Expand Down
2 changes: 1 addition & 1 deletion hakoniwa/config/sample_control/drone_config_2.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
"physicsEquation": "BodyFrame",
"collision_detection": true,
"manual_control": false,
"airFrictionCoefficient": [ 0.0001, 0.0 ],
"airFrictionCoefficient": [ 0.02, 0.0 ],
"inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
"mass_kg": 0.1,
"body_size": [ 0.1, 0.1, 0.01 ],
Expand Down
2 changes: 1 addition & 1 deletion hakoniwa/config/sample_control/drone_config_3.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
"physicsEquation": "BodyFrame",
"collision_detection": true,
"manual_control": false,
"airFrictionCoefficient": [ 0.0001, 0.0 ],
"airFrictionCoefficient": [ 0.02, 0.0 ],
"inertia": [ 0.0000625, 0.00003125, 0.00009375 ],
"mass_kg": 0.1,
"body_size": [ 0.1, 0.1, 0.01 ],
Expand Down
90 changes: 76 additions & 14 deletions hakoniwa/src/hako/pdu/hako_pdu_accessor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,24 +54,34 @@ static inline void debug_print_drone_collision(hako::assets::drone::DroneDynamic
static inline void do_io_read_control(hako::assets::drone::IAirCraft *drone, DroneThrustType& thrust, DroneTorqueType& torque)
{
Hako_Twist control;
char buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(Hako_Twist)];

if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_CTRL, (char*)&control, sizeof(control))) {
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_CTRL, buffer, sizeof(buffer))) {
std::cerr << "ERROR: can not read pdu data: control" << std::endl;
return;
}
if (hako_pdu_get_fixed_data(buffer, (char*)&control, sizeof(Hako_Twist), sizeof(buffer)) < 0) {
std::cerr << "ERROR: can not get pdu data: control" << std::endl;
return;
}
thrust.data = control.linear.z;
torque.data.x = control.angular.x;
torque.data.y = control.angular.y;
torque.data.z = control.angular.z;
return;
}

static inline void do_io_read_collision(hako::assets::drone::IAirCraft *drone, hako::assets::drone::DroneDynamicsCollisionType& drone_collision)
{
Hako_Collision hako_collision;
char buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(Hako_Collision)];
memset(&drone_collision, 0, sizeof(drone_collision));
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_COLLISION, (char*)&hako_collision, sizeof(hako_collision))) {
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_COLLISION, buffer, sizeof(buffer))) {
std::cerr << "ERROR: can not read pdu data: Hako_Collision" << std::endl;
return;
}
if (hako_pdu_get_fixed_data(buffer, (char*)&hako_collision, sizeof(Hako_Collision), sizeof(buffer)) < 0) {
std::cerr << "ERROR: can not get pdu data: Hako_Collision" << std::endl;
return;
}
drone_collision.collision = (bool)hako_collision.collision;
if (drone_collision.collision) {
Expand All @@ -87,17 +97,31 @@ static inline void do_io_read_collision(hako::assets::drone::IAirCraft *drone, h
}
debug_print_drone_collision(drone_collision);
hako_collision.collision = false;
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_COLLISION, (const char*)&hako_collision, sizeof(hako_collision))) {

// 書き込み用バッファを準備する
char write_buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(Hako_Collision)];
if (hako_pdu_put_fixed_data(write_buffer, reinterpret_cast<const char*>(&hako_collision), sizeof(Hako_Collision), sizeof(write_buffer)) < 0) {
std::cerr << "ERROR: can not put pdu data: Hako_Collision" << std::endl;
return;
}
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_COLLISION, write_buffer, sizeof(write_buffer))) {
std::cerr << "ERROR: can not write pdu data: Hako_Collision" << std::endl;
}
}
}

static inline void do_io_read_manual(hako::assets::drone::IAirCraft *drone, hako::assets::drone::DroneDynamicsManualControlType& drone_manual)
{
Hako_ManualPosAttControl hako_manual;
char buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(Hako_ManualPosAttControl)];
memset(&hako_manual, 0, sizeof(hako_manual));
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_MANUAL, (char*)&hako_manual, sizeof(hako_manual))) {
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_MANUAL, buffer, sizeof(buffer))) {
std::cerr << "ERROR: can not read pdu data: Hako_ManualPosAttControl" << std::endl;
return;
}
if (hako_pdu_get_fixed_data(buffer, (char*)&hako_manual, sizeof(Hako_ManualPosAttControl), sizeof(buffer)) < 0) {
std::cerr << "ERROR: can not get pdu data: Hako_ManualPosAttControl" << std::endl;
return;
}
drone_manual.control = hako_manual.do_operation;
if (drone_manual.control) {
Expand All @@ -109,7 +133,14 @@ static inline void do_io_read_manual(hako::assets::drone::IAirCraft *drone, hako
drone_manual.pos.data.x = hako_manual.posatt.linear.x;
drone_manual.pos.data.y = hako_manual.posatt.linear.y;
drone_manual.pos.data.z = hako_manual.posatt.linear.z;
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_MANUAL, (const char*)&hako_manual, sizeof(hako_manual))) {

// 書き込み用バッファを準備する
char write_buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(Hako_ManualPosAttControl)];
if (hako_pdu_put_fixed_data(write_buffer, reinterpret_cast<const char*>(&hako_manual), sizeof(Hako_ManualPosAttControl), sizeof(write_buffer)) < 0) {
std::cerr << "ERROR: can not put pdu data: Hako_ManualPosAttControl" << std::endl;
return;
}
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_MANUAL, write_buffer, sizeof(write_buffer))) {
std::cerr << "ERROR: can not write pdu data: Hako_ManualPosAttControl" << std::endl;
}
}
Expand All @@ -118,27 +149,45 @@ static inline void do_io_read_manual(hako::assets::drone::IAirCraft *drone, hako
static inline void do_io_read_disturb(hako::assets::drone::IAirCraft *drone, hako::assets::drone::DroneDynamicsDisturbanceType& drone_disturb)
{
Hako_Disturbance hako_disturb;
char buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(Hako_Disturbance)];
memset(&hako_disturb, 0, sizeof(hako_disturb));
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), HAKO_AVATOR_CHANNEL_ID_DISTURB, (char*)&hako_disturb, sizeof(hako_disturb))) {
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), HAKO_AVATOR_CHANNEL_ID_DISTURB, buffer, sizeof(buffer))) {
std::cerr << "ERROR: can not read pdu data: Hako_Disturbance" << std::endl;
return;
}
if (hako_pdu_get_fixed_data(buffer, (char*)&hako_disturb, sizeof(Hako_Disturbance), sizeof(buffer)) < 0) {
std::cerr << "ERROR: can not get pdu data: Hako_Disturbance" << std::endl;
return;
}
//temperature
drone_disturb.values.d_temp.value = hako_disturb.d_temp.value;
}
#include <iomanip> // for std::hex and std::setfill

template<typename PacketType>
static inline bool do_io_read_cmd(hako::assets::drone::IAirCraft *drone, int channel_id, PacketType& packet) {
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), channel_id, reinterpret_cast<char*>(&packet), sizeof(packet))) {
std::cerr << "ERROR: can not read pdu data: packet channel_id: " << channel_id << std::endl;
static inline bool do_io_read_cmd(hako::assets::drone::IAirCraft *drone, int channel_id, PacketType& packet)
{
char buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(PacketType)];
if (!hako_asset_runner_pdu_read(drone->get_name().c_str(), channel_id, buffer, sizeof(buffer))) {
std::cerr << "ERROR: can not read pdu data: packet channel_id: " << channel_id << std::endl;
return false;
}
if (hako_pdu_get_fixed_data(buffer, (char*)&packet, sizeof(PacketType), sizeof(buffer)) < 0) {
//std::cerr << "ERROR: can not get pdu data: PacketType" << std::endl;
return false;
}
return true;
}

template<typename PacketType>
static inline bool do_io_write_cmd(hako::assets::drone::IAirCraft *drone, int channel_id, const PacketType& packet) {
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), channel_id, reinterpret_cast<const char*>(&packet), sizeof(packet))) {
std::cerr << "ERROR: can not write pdu data: packet channel_id: " << channel_id << std::endl;
char buffer[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(PacketType)];
if (hako_pdu_put_fixed_data(buffer, reinterpret_cast<const char*>(&packet), sizeof(PacketType), sizeof(buffer)) < 0) {
std::cerr << "ERROR: can not put pdu data: PacketType" << std::endl;
return false;
}
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), channel_id, buffer, sizeof(buffer))) {
std::cerr << "ERROR: can not write pdu data: packet channel_id: " << channel_id << std::endl;
return false;
}
return true;
Expand All @@ -148,13 +197,20 @@ static inline void do_io_write(hako::assets::drone::IAirCraft *drone, const doub
{
Hako_HakoHilActuatorControls hil_actuator_controls;
Hako_Twist pos;
char buffer_hil_actuator[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(Hako_HakoHilActuatorControls)];
char buffer_pos[HAKO_PDU_FIXED_DATA_SIZE_BY_TYPE(Hako_Twist)];

memset(&hil_actuator_controls, 0, sizeof(hil_actuator_controls));
for (int i = 0; i < hako::assets::drone::ROTOR_NUM; i++) {
hil_actuator_controls.controls[i] = controls[i];
}
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_MOTOR, (const char*)&hil_actuator_controls, sizeof(hil_actuator_controls))) {
if (hako_pdu_put_fixed_data(buffer_hil_actuator, reinterpret_cast<const char*>(&hil_actuator_controls), sizeof(Hako_HakoHilActuatorControls), sizeof(buffer_hil_actuator)) < 0) {
std::cerr << "ERROR: can not put pdu data: hil_actuator_controls" << std::endl;
return;
}
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_MOTOR, buffer_hil_actuator, sizeof(buffer_hil_actuator))) {
std::cerr << "ERROR: can not write pdu data: hil_actuator_controls" << std::endl;
return;
}

DronePositionType dpos = drone->get_drone_dynamics().get_pos();
Expand All @@ -165,8 +221,14 @@ static inline void do_io_write(hako::assets::drone::IAirCraft *drone, const doub
pos.angular.x = dangle.data.x;
pos.angular.y = -dangle.data.y;
pos.angular.z = -dangle.data.z;
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_POS, (const char*)&pos, sizeof(pos))) {

if (hako_pdu_put_fixed_data(buffer_pos, reinterpret_cast<const char*>(&pos), sizeof(Hako_Twist), sizeof(buffer_pos)) < 0) {
std::cerr << "ERROR: can not put pdu data: pos" << std::endl;
return;
}
if (!hako_asset_runner_pdu_write(drone->get_name().c_str(), HAKO_AVATOR_CHANNLE_ID_POS, buffer_pos, sizeof(buffer_pos))) {
std::cerr << "ERROR: can not write pdu data: pos" << std::endl;
return;
}
}

Expand Down
2 changes: 1 addition & 1 deletion hakoniwa/third-party/hakoniwa-core-cpp-client
Submodule hakoniwa-core-cpp-client updated 24 files
+25 −34 .github/workflows/build.yml
+1 −1 README.md
+41 −0 bindings/python/hako_binary/binary_io.py
+33 −8 bindings/python/hako_binary/binary_reader.py
+90 −25 bindings/python/hako_binary/binary_writer.py
+1 −1 bindings/python/hako_binary/offset/ev3_msgs/Ev3PduActuatorHeader.offset
+4 −0 bindings/python/hako_binary/offset/hako_mavlink_msgs/HakoHilActuatorControls.offset
+15 −0 bindings/python/hako_binary/offset/hako_mavlink_msgs/HakoHilGps.offset
+16 −0 bindings/python/hako_binary/offset/hako_mavlink_msgs/HakoHilSensor.offset
+16 −0 bindings/python/hako_binary/offset/hako_mavlink_msgs/HakoHilStateQuaternion.offset
+5 −0 bindings/python/hako_binary/offset/hako_msgs/SimpleStructVarray.offset
+3 −0 bindings/python/hako_binary/offset/hako_msgs/SimpleVarray.offset
+4 −0 bindings/python/hako_binary/offset/pico_msgs/LightSensor.offset
+4 −0 bindings/python/hako_binary/offset/pico_msgs/PicoSensor.offset
+3 −0 bindings/python/hako_binary/offset/satelite_msgs/SateliteImu.offset
+2 −2 bindings/python/hako_binary/offset/sensor_msgs/LaserScan.offset
+8 −0 bindings/python/hako_binary/offset_parser.py
+1 −0 bindings/python/hako_pdu.py
+1 −1 core
+4 −4 examples/unity_sample/custom.json
+7 −28 install.bash
+7 −0 test/config.json
+45 −0 test/hakoAsset-01/CMakeLists.txt
+72 −0 test/hakoAsset-01/test.cpp
2 changes: 1 addition & 1 deletion hakoniwa/third-party/hakoniwa-ros2pdu
Submodule hakoniwa-ros2pdu updated 106 files

0 comments on commit 8fbed6b

Please sign in to comment.