From 85d03b9603efed971e400249cef4aec9eaa7d021 Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 29 Mar 2026 19:44:37 +0100 Subject: [PATCH 1/2] Refactor turtle_bot (#179) --- examples/example_14/example_14.cpp | 381 +++--------------- robots/bitrl_diff_drive_robot.json | 107 +++-- ...t_chassis.cpp => turtle_robot_chassis.cpp} | 9 +- ...robot_chassis.h => turtle_robot_chassis.h} | 17 +- ...e_robot_part.cpp => turtle_robot_part.cpp} | 2 +- ...drive_robot_part.h => turtle_robot_part.h} | 15 +- ...bot_plates.cpp => turtle_robot_plates.cpp} | 4 +- ...e_robot_plates.h => turtle_robot_plates.h} | 11 +- ...e_robot_rods.cpp => turtle_robot_rods.cpp} | 2 +- ...drive_robot_rods.h => turtle_robot_rods.h} | 6 +- ...bot_wheels.cpp => turtle_robot_wheels.cpp} | 4 +- ...e_robot_wheels.h => turtle_robot_wheels.h} | 6 +- .../chrono_robots/tmp/chrono_robot_pose.h | 84 ---- .../tmp/org_chrono_diff_drive_robot.cpp | 289 ------------- .../tmp/org_chrono_diff_drive_robot.h | 179 -------- .../chrono_robots/tmp/rb_chrono_module.h | 12 - ...{diff_drive_robot.cpp => turtle_robot.cpp} | 20 +- .../{diff_drive_robot.h => turtle_robot.h} | 41 +- .../backends/chrono_ultrasound_backend.cpp | 26 +- .../backends/chrono_ultrasound_backend.h | 25 +- .../backends/mqtt_ultrasound_backend.cpp | 6 + .../backends/mqtt_ultrasound_backend.h | 13 +- .../backends/range_sensor_backend_base.h | 21 +- .../sensors/backends/sensor_backend_base.h | 28 ++ src/bitrl/sensors/sensor_base.h | 9 + src/bitrl/sensors/sensor_manager.h | 9 + src/bitrl/sensors/sensor_type_enum.h | 2 +- src/bitrl/sensors/ultrasonic_sensor.cpp | 7 + src/bitrl/sensors/ultrasonic_sensor.h | 9 + src/bitrl/utils/io/csv_file_reader.cpp | 9 +- src/bitrl/utils/io/csv_file_reader.h | 2 - src/bitrl/utils/io/file_handler_base.h | 153 +++++-- src/bitrl/utils/io/file_reader_base.cpp | 14 +- src/bitrl/utils/io/json_file_reader.cpp | 9 +- src/bitrl/utils/io/json_file_reader.h | 5 + 35 files changed, 469 insertions(+), 1067 deletions(-) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_chassis.cpp => turtle_robot_chassis.cpp} (95%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_chassis.h => turtle_robot_chassis.h} (80%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_part.cpp => turtle_robot_part.cpp} (97%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_part.h => turtle_robot_part.h} (95%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_plates.cpp => turtle_robot_plates.cpp} (99%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_plates.h => turtle_robot_plates.h} (94%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_rods.cpp => turtle_robot_rods.cpp} (98%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_rods.h => turtle_robot_rods.h} (94%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_wheels.cpp => turtle_robot_wheels.cpp} (98%) rename src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/{diff_drive_robot_wheels.h => turtle_robot_wheels.h} (94%) delete mode 100644 src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h delete mode 100644 src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp delete mode 100644 src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h delete mode 100644 src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h rename src/bitrl/rigid_bodies/chrono_robots/{diff_drive_robot.cpp => turtle_robot.cpp} (97%) rename src/bitrl/rigid_bodies/chrono_robots/{diff_drive_robot.h => turtle_robot.h} (71%) diff --git a/examples/example_14/example_14.cpp b/examples/example_14/example_14.cpp index 7247248..8fcb9d8 100644 --- a/examples/example_14/example_14.cpp +++ b/examples/example_14/example_14.cpp @@ -1,18 +1,16 @@ #include "bitrl/bitrl_config.h" -#ifdef BITRL_CHRONO +#if defined(BITRL_CHRONO) && defined(BITRL_IRRLICHT) #include "bitrl/bitrl_consts.h" #include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" -#include "bitrl/envs/env_base.h" -#include "bitrl/envs/time_step.h" -#include "bitrl/envs/time_step_type.h" -#include "bitrl/envs/env_types.h" #include "bitrl/utils/bitrl_utils.h" -#include -#include +#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" +#include "bitrl/utils/render/irrlicht_utils.h" +#include +//#include #include #ifdef BITRL_LOG @@ -23,346 +21,95 @@ #include #include #include +#include namespace example14 { using namespace bitrl; -using namespace chrono::irrlicht; - -// constants we will be using further below -const uint_t WINDOW_HEIGHT = 800; -const uint_t WINDOW_WIDTH = 1024; -const real_t DT = 0.0001; -const real_t SIM_TIME = 5.0; -const std::string WINDOW_TITLE( "Example 14"); - -constexpr uint_t STATE_SPACE_SIZE = 2; -constexpr uint_t ACTION_SPACE_SIZE = 1; - -using bitrl::TimeStepTp; -using bitrl::TimeStep; - -//typedef TimeStep > time_step_type; -typedef TimeStep time_step_type; -typedef bitrl::envs::ContinuousVectorStateContinuousVectorActionEnv space_type; - -std::shared_ptr build_wheel(const std::string &wheel_label, real_t radius, - real_t width, const chrono::ChVector3d& pos); - -void draw_world_axes(chrono::irrlicht::ChVisualSystemIrrlicht& vis, - double scale = 1.0) { - auto* driver = vis.GetVideoDriver(); - - // X axis (red) - driver->draw3DLine( - {0, 0, 0}, - {static_cast(scale), 0, 0}, - irr::video::SColor(255, 255, 0, 0)); - - // Y axis (green) - driver->draw3DLine( - {0, 0, 0}, - {0, static_cast(scale), 0}, - irr::video::SColor(255, 0, 255, 0)); - - // Z axis (blue) - driver->draw3DLine( - {0, 0, 0}, - {0, 0, static_cast(scale)}, - irr::video::SColor(255, 0, 0, 255)); -} - -// class to model the robot -class DiffDriveRobot -{ -public: - - // add the components of this robot to the system we simulate - void add_to_sys(chrono::ChSystemSMC& sys); - - // build the robot - void build(); - - // set the speeds of the motors - void set_speed(real_t speed); - - // reset the robot - void reset(); - - // the pose of the robot - bitrl::rb::bitrl_chrono::CHRONO_RobotPose& pose()noexcept{return pose_;} - -private: - - //The chassis of the robot - std::shared_ptr chassis_; - std::pair, std::shared_ptr> wheels_; - std::shared_ptr caster_wheel_; - bitrl::rb::bitrl_chrono::CHRONO_RobotPose pose_; - -}; - -void DiffDriveRobot::build() -{ - // build the chassis of the robot - chassis_ = chrono_types::make_shared(); - chassis_->SetMass(1.0); - chassis_->SetPos(chrono::ChVector3d(0.0, 0.0, 0.22)); +using bitrl::rb::bitrl_chrono::CHRONO_DiffDriveRobot; - // allow the chassis to move - chassis_->SetFixed(false); - - // add visual shape for visualization - auto vis_shape = chrono_types::make_shared( - chrono::ChVector3d(0.4, 0.3, 0.05)); - chassis_ -> AddVisualShape(vis_shape); - - // build the wheels of the robot - wheels_.first = build_wheel("left_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, 0.175, 0.16)); - wheels_.second = build_wheel("right_wheel", 0.06, 0.05, chrono::ChVector3d(0.0, -0.175, 0.16)); - caster_wheel_ = build_wheel("caster_wheel", 0.06, 0.05, chrono::ChVector3d(0.2, 0.0, 0.16)); - - // we want to tract the chassis pose - pose_.set_body(chassis_); - -} - -void -DiffDriveRobot::reset() -{ - chassis_ -> SetPos(chrono::ChVector3d(0.0, 0.0, 0.22)); - wheels_.first -> SetPos(chrono::ChVector3d(0.0, 0.175, 0.16)); - wheels_.second -> SetPos(chrono::ChVector3d(0.0, -0.175, 0.16)); - caster_wheel_ -> SetPos(chrono::ChVector3d(0.2, 0.0, 0.16)); -} - -std::shared_ptr build_wheel(const std::string &wheel_label, real_t radius, - real_t width, const chrono::ChVector3d& pos) -{ - - // rotation axis for the wheel - chrono::ChQuaternion<> q; - q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); - - auto wheel = chrono_types::make_shared(); - wheel->SetMass(1.0); - wheel->SetPos(pos); - wheel->SetName(wheel_label); - wheel->SetRot(chrono::QUNIT); - wheel->SetFixed(false); - - auto visual_shape = chrono_types::make_shared(radius, width); - - chrono::ChQuaterniond qvis; - qvis.SetFromAngleAxis(chrono::CH_PI_2, chrono::VECT_X); - - chrono::ChFrame<> vis_frame(chrono::VNULL, qvis); - - wheel->AddVisualShape(visual_shape, vis_frame); - return wheel; -} - -void DiffDriveRobot::add_to_sys(chrono::ChSystemSMC& sys) +auto build_generic_material() { - sys.Add(chassis_); - sys.Add(wheels_.first); - sys.Add(wheels_.second); - sys.Add(caster_wheel_); - -} - -void DiffDriveRobot::set_speed(real_t speed) -{ - chassis_ -> SetAngVelLocal(chrono::VNULL); - chassis_ -> SetAngAccLocal(chrono::VNULL); - chassis_ -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - wheels_.first -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - wheels_.second -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - caster_wheel_ -> SetLinVel(chrono::ChVector3d(speed, 0.0, 0.0)); - + auto material = chrono_types::make_shared(); + material->SetFriction(0.8f); + material->SetRestitution(0.1f); + return material; } -class DiffDriveRobotEnv final: public bitrl::envs::EnvBase -{ -public: - - - typedef typename space_type::action_type action_type; - - DiffDriveRobotEnv(); - - virtual void make(const std::string &version, - const std::unordered_map &make_options, - const std::unordered_map &reset_options) override; - - virtual void close()override{} - - virtual time_step_type reset()override; - virtual time_step_type step(const action_type &/*action*/)override; - - void simulate(); - -private: - - DiffDriveRobot robot_; - chrono::ChSystemSMC sys_; - uint_t sim_counter_{0}; - real_t current_time_{0.0}; - void build_system_(); - -}; - -DiffDriveRobotEnv::DiffDriveRobotEnv() - : -bitrl::envs::EnvBase(bitrl::utils::uuid4(), "DiffDriveRobotEnv"), -robot_(), -sys_() -{} - -void DiffDriveRobotEnv::build_system_() +auto build_floor(std::shared_ptr material) { - // create material for the ground - auto material = chrono_types::make_shared(); - - material->SetYoungModulus(2e7); // stiffness (important) - material->SetPoissonRatio(0.3); - material->SetFriction(0.9f); // traction - material->SetRestitution(0.0f); // no bouncing - - // Optional but recommended - material->SetAdhesion(0.0); - material->SetKn(2e5); // normal stiffness override - material->SetGn(40.0); // normal damping - material->SetKt(2e5); // tangential stiffness - material->SetGt(20.0); - - auto ground = chrono_types::make_shared( - 5.0, 5.0, 0.2, - 1000, - true, // visual - true, // collision - material - ); - ground->SetFixed(true); - ground->SetPos({0, 0, -0.1}); - - // set the gravity acceleration - sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -bitrl::consts::maths::G)); - sys_.Add(ground); + auto floor = chrono_types::make_shared( + 5, 5, 0.1, // size + 1000, // density + true, true, material); // visual + collision + + floor->SetPos(chrono::ChVector3d(0,0,-0.05)); + floor->SetFixed(true); + floor->EnableCollision(true); + return floor; } -void -DiffDriveRobotEnv::make(const std::string &version, - const std::unordered_map &make_options, - const std::unordered_map &reset_options) -{ - - robot_.build(); - build_system_(); - - robot_.add_to_sys(sys_); - - this -> set_make_options_(make_options); - this -> set_reset_options_(reset_options); - this -> set_version_(version); - this -> make_created_(); - } -time_step_type -DiffDriveRobotEnv::reset() +int main() { - sim_counter_ = 1; - current_time_ = 0.0; - robot_.reset(); - robot_.set_speed(15.0); - - auto robot_position =robot_.pose().position(); - return time_step_type(TimeStepTp::FIRST, 0.0, robot_position); -} + using namespace example14; -time_step_type -DiffDriveRobotEnv::step(const action_type &/*action*/) -{ - if (sim_counter_ % 100 == 0) - { #ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Reset simulation: "; + BOOST_LOG_TRIVIAL(info)<<"Starting simulation...: "; #endif - return reset(); - } - - sys_.DoStepDynamics(DT); - auto robot_position =robot_.pose().position(); - - sim_counter_++; - current_time_ += DT; - return time_step_type(TimeStepTp::MID, 1.0, robot_position); -} - - - -void DiffDriveRobotEnv::simulate() -{ - // create the object that handles the visualization - chrono::irrlicht::ChVisualSystemIrrlicht visual; - visual.AttachSystem(&sys_); - visual.SetWindowSize(WINDOW_WIDTH, WINDOW_WIDTH); //WINDOW_HEIGHT); - visual.SetWindowTitle(WINDOW_TITLE); - visual.Initialize(); - draw_world_axes(visual); - visual.AddLogo(); - visual.AddSkyBox(); - visual.AddCamera({0, -2, 1}, {0, 0, 0}); - visual.AddTypicalLights(); - visual.BindAll(); - - // we need this - while (visual.Run()) - { - - // Irrlicht must prepare frame to draw - visual.BeginScene(); - // .. draw items belonging to Irrlicht scene, if any - visual.Render(); + // create the system and attach the robot to it + chrono::ChSystemNSC system; + system.SetGravitationalAcceleration(chrono::ChVector3d(0,0,-bitrl::consts::maths::G)); + system.SetCollisionSystemType(chrono::ChCollisionSystem::Type::BULLET); - // .. draw a grid - tools::drawGrid(&visual, 0.5, 0.5); - draw_world_axes(visual, 1.5); + auto floor = build_floor(build_generic_material()); + system.Add(floor); - auto time_step = step( action_type()); #ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"At time: "< GetPos(); + //std::cout << position <(), - std::unordered_map()); - env.reset(); - - env.simulate(); - env.close(); return 0; } diff --git a/robots/bitrl_diff_drive_robot.json b/robots/bitrl_diff_drive_robot.json index cd32367..7c4dfa2 100644 --- a/robots/bitrl_diff_drive_robot.json +++ b/robots/bitrl_diff_drive_robot.json @@ -1,45 +1,84 @@ { "name": "Odysseus", - "mass": 1.0, - "mass_units": "kg", + "robot-data": { + "max_speed": 5.0, + "wheel_radius": 0.1, + "wheel_width": 0.05, + "axle_length": 1.0, + "density": 1000.0, + "caster_wheel_radius": 0.05 + }, + "material": { + "friction": 0.8, + "restitution": 0.1 + }, "chassis": { - "mass": 1.0, - "mass_units": "kg", - "position": [ 0.0, 0.0, 0.0], - "fixed": false, - "visual_shape": "shape-box", - "visual_position": [0.4, 0.3, 0.05] + "description": "chassis_i describes the length of the chassis in the x, y, z axis, pos_i the center of mass of the chassis", + "chassis_x": 0.5, + "chassis_y": 0.3, + "chassis_z": 0.1, + "pos_x": 0.0, + "pos_y": 0.0, + "pos_z": 0.0 }, "left-wheel": { - "mass": 1.0, - "mass_units": "kg", - "position": [ 0.0, 0.175, -0.02], - "radius": 0.06, - "width": 0.05, - "radius_units": "cm", - "visual_shape": "cylinder", - "rotation_axis": [1, 0, 0], - "rotation_angle_deg": 90 + "pos_x": 0.0, + "pos_y": 0.2, + "pos_z": 0.1 }, "right-wheel": { - "mass": 1.0, - "mass_units": "kg", - "position": [ 0.0, -0.175, -0.02], - "radius": 0.06, - "width": 0.05, - "radius_units": "cm", - "visual_shape": "cylinder", - "rotation_axis": [1, 0, 0], - "rotation_angle_deg": 90 + "pos_x": 0.0, + "pos_y": -0.2, + "pos_z": 0.1 }, "caster-wheel": { - "mass": 0.5, - "mass_units": "kg", - "position": [ -0.2, 0.0, -0.02], - "radius": 0.06, - "width": 0.05, - "visual_shape": "cylinder", - "rotation_axis": [1, 0, 0], - "rotation_angle_deg": 90 + "pos_x": 0.2, + "pos_y": 0.0, + "pos_z": 0.05 + }, + "sensors": { + "description": "The sensors of the robot. Leave the sensors attribute empty if no sensors. Or set all flags enble to false", + "sensors": { + "front-sonar": { + "idx": 0, + "enable": true, + "dimensions": [0.04, 0.04, 0.04], + "update_rate": 50.0, + "max_distance": 5.0, + "min_distance": 0.5, + "backend": "CHRONO", + "type": "ultrasonic-range", + "name": "Ultrasonic", + "color": [1.0, 0.0, 0.0], + "frame_pos": [0.27, 0.0, 0.05] + + }, + "rear-sonar": { + "idx": 1, + "enable": true, + "pos_x": 0.0, + "pos_y": 0.0, + "pos_z": 0.0, + "update_rate": 50.0, + "max_distance": 5.0, + "min_distance": 0.5, + "backend": "CHRONO", + "type": "ultrasonic-range", + "name": "Ultrasonic" + }, + "front-camera": { + "idx": 2, + "enable": true, + "pos_x": 0.0, + "pos_y": 0.0, + "pos_z": 0.0, + "update_rate": 50.0, + "max_distance": 5.0, + "min_distance": 0.5, + "backend": "CHRONO", + "type": "camera", + "name": "Ultrasonic" + } + } } } \ No newline at end of file diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.cpp similarity index 95% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.cpp index ca2324d..b2ae7a6 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.cpp @@ -2,12 +2,13 @@ #ifdef BITRL_CHRONO +#include "bitrl/bitrl_consts.h" +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.h" + +#include -#include "chrono/assets/ChColor.h" -#include "bitrl/bitrl_types.h" -#include "bitrl/bitrl_consts.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h" namespace bitrl{ namespace rb::bitrl_chrono diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.h similarity index 80% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.h index 1f36a2a..e2ae74e 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.h @@ -1,22 +1,15 @@ -// -// Created by alex on 2/8/26. -// - -#ifndef DIFF_DRIVE_ROBOT_CHASSIS_H -#define DIFF_DRIVE_ROBOT_CHASSIS_H +#ifndef TURTLE_ROBOT_CHASSIS_H +#define TURTLE_ROBOT_CHASSIS_H #include "bitrl/bitrl_config.h" #ifdef BITRL_CHRONO -#include -#include - -#include "chrono/physics/ChSystem.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" +#include -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" -#include "bitrl/bitrl_types.h" +#include namespace bitrl{ namespace rb::bitrl_chrono{ diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.cpp similarity index 97% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.cpp index fd87d75..5e5eb99 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.cpp @@ -2,8 +2,8 @@ #ifdef BITRL_CHRONO -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" namespace bitrl{ namespace rb::bitrl_chrono{ diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h similarity index 95% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h index ec4c281..7cb3086 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h @@ -1,18 +1,19 @@ -#ifndef DIFF_DRIVE_ROBOT_PART_H -#define DIFF_DRIVE_ROBOT_PART_H +#ifndef TURTLE_ROBOT_PART_H +#define TURTLE_ROBOT_PART_H #include "bitrl/bitrl_config.h" #ifdef BITRL_CHRONO -#include -#include + #include "bitrl/bitrl_types.h" -#include "chrono/assets/ChColor.h" -#include "chrono/physics/ChSystem.h" -#include "chrono/physics/ChLinkMotorRotationSpeed.h" +#include +#include + +#include +#include namespace bitrl{ namespace rb::bitrl_chrono{ diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.cpp similarity index 99% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.cpp index 654395d..d6c79de 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.cpp @@ -2,9 +2,9 @@ #ifdef BITRL_CHRONO -#include "bitrl/bitrl_types.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.h" namespace bitrl { diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.h similarity index 94% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.h index ed45053..30a3c10 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.h @@ -1,16 +1,15 @@ -#ifndef DIFF_DRIVE_ROBOT_PLATES_H -#define DIFF_DRIVE_ROBOT_PLATES_H +#ifndef TURTLE_ROBOT_PLATES_H +#define TURTLE_ROBOT_PLATES_H #include "bitrl/bitrl_config.h" #ifdef BITRL_CHRONO -#include -#include -#include "chrono/physics/ChSystem.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" +#include +#include namespace bitrl { diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.cpp similarity index 98% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.cpp index effafc5..0c2085a 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.cpp @@ -2,8 +2,8 @@ #ifdef BITRL_CHRONO -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.h" namespace bitrl { diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.h similarity index 94% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.h index dedf3ae..0c488b9 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.h @@ -1,5 +1,5 @@ -#ifndef DIFF_DRIVE_ROBOT_RODS_H -#define DIFF_DRIVE_ROBOT_RODS_H +#ifndef TURTLE_ROBOT_RODS_H +#define TURTLE_ROBOT_RODS_H #include "bitrl/bitrl_config.h" @@ -10,7 +10,7 @@ #include "chrono/physics/ChSystem.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" namespace bitrl { diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.cpp similarity index 98% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.cpp index 8738eca..5213d72 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.cpp @@ -2,9 +2,9 @@ #ifdef BITRL_CHRONO -#include "chrono/assets/ChColor.h" #include "bitrl/bitrl_types.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.h" +#include "chrono/assets/ChColor.h" #include "bitrl/bitrl_consts.h" diff --git a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.h similarity index 94% rename from src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h rename to src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.h index d2399b6..4941c47 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h +++ b/src/bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.h @@ -1,5 +1,5 @@ -#ifndef DIFF_DRIVE_ROBOT_WHEELS_H -#define DIFF_DRIVE_ROBOT_WHEELS_H +#ifndef TURTLE_ROBOT_WHEELS_H +#define TURTLE_ROBOT_WHEELS_H #include "bitrl/bitrl_config.h" @@ -8,7 +8,7 @@ #include #include -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_part.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_part.h" namespace bitrl{ namespace rb::bitrl_chrono { diff --git a/src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h deleted file mode 100644 index 2806005..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/tmp/chrono_robot_pose.h +++ /dev/null @@ -1,84 +0,0 @@ -#ifndef CHRONO_ROBOT_POSE_H -#define CHRONO_ROBOT_POSE_H - -#include "bitrl/bitrl_config.h" - -#ifdef BITRL_CHRONO -#include "bitrl/bitrl_types.h" -#include -#include -namespace bitrl -{ -namespace rb::bitrl_chrono -{ - -/** - * @class CHRONO_RobotPose - * @ingroup rb_chrono - * @brief Wrapper for representing the state of a CHRONO_Robot - */ -class CHRONO_RobotPose -{ -public: - - /** - *@brief Constructor. Initialize by passing a pointer to the chrono::ChBody - * to monitor - */ - explicit CHRONO_RobotPose(std::shared_ptr robot_ptr=nullptr); - - /** - * Set/Reset the chrono::ChBody to monitor - * @param robot_ptr - */ - void set_body(std::shared_ptr robot_ptr){robot_ptr_ = robot_ptr;} - - /** - * @brief Return the position vector. This is the position of the CoM of the body - */ - const chrono::ChVector3d& position() const {return robot_ptr_ -> GetPos();} - - /** - * @return The linear velocity of the monitored body - */ - const chrono::ChVector3d& velocity()const {return robot_ptr_ -> GetLinVel();} - - /** - * @return The linear velocity of the monitored body - */ - const chrono::ChVector3d& acceleration()const {return robot_ptr_ -> GetLinAcc();} - - /** - * @brief Return the rotation matrix - */ - const chrono::ChMatrix33d& rotation_matrix() const {return robot_ptr_ -> GetRotMat();} - - /** - * @brief Return the transformation of the local to the world coordinats - */ - chrono::ChVector3d to_world_coords(chrono::ChVector3d point){return robot_ptr_ -> GetFrameRefToAbs().TransformPointLocalToParent(point);} - - /** - * @brief Transform the world point to local coordinates - */ - chrono::ChVector3d to_local_coords(chrono::ChVector3d point){return robot_ptr_ -> GetFrameRefToAbs().TransformPointParentToLocal(point);} -private: - - /** - * @brief Pointer to the chrono::ChBody to monitor - */ - std::shared_ptr robot_ptr_; - -}; - -inline -CHRONO_RobotPose::CHRONO_RobotPose(std::shared_ptr robot_ptr) - : -robot_ptr_(robot_ptr) -{} -} -} - -#endif - -#endif //CHRONO_ROBOT_STATE_H diff --git a/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp deleted file mode 100644 index e85c28d..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.cpp +++ /dev/null @@ -1,289 +0,0 @@ -#include "bitrl/bitrl_config.h" - -#ifdef BITRL_CHRONO - -#include "bitrl/rigid_bodies/chrono_robots/chrono_diff_drive_robot.h" -#include "bitrl/bitrl_consts.h" -#include "bitrl/utils/io/json_file_reader.h" -#include "bitrl/extern/nlohmann/json/json.hpp" - -#include -#include -#include -#include -#include - - -#ifdef BITRL_LOG -#include -#endif - -#include -#include -#include -#include - - -namespace bitrl -{ -namespace rb::bitrl_chrono -{ -namespace -{ - -using json = nlohmann::json; - -// helper class to read chassis -struct Chassis -{ - std::array position; - std::array visual_position; - std::string mass_units; - std::string visual_shape; - real_t mass; - bool fixed; - - Chassis(const json &j); -}; - -Chassis::Chassis(const json &j) - : -mass(j["mass"].get()), -mass_units(j["mass_units"].get()), -fixed(j["fixed"].get()), -visual_shape(j["visual_shape"].get()), -position(), -visual_position() -{ - auto pos = j.at("position"); - for (size_t i = 0; i < 3; ++i) - position[i] = pos.at(i).get(); - - auto vis_position = j.at("visual_position"); - for (size_t i = 0; i < 3; ++i) - visual_position[i] = vis_position.at(i).get(); -} - -// helper struct to read a wheel -struct Wheel -{ - std::array position; - std::string mass_units; - std::string visual_shape; - real_t mass; - real_t width; - real_t radius; - - Wheel(const json &j); -}; - -Wheel::Wheel(const json &j) - : -position(), -mass_units(j["mass_units"].get()), -visual_shape(j["visual_shape"].get()), -mass(j["mass"].get()), -width(j["width"].get()), -radius(j["radius"].get()) - -{ - auto pos = j.at("position"); - for (size_t i = 0; i < 3; ++i) - position[i] = pos.at(i).get(); -} - - -auto build_chassis(const utils::io::JSONFileReader& json_reader) -{ - auto chassis_data = json_reader.template at("chassis"); - auto chassis = chrono_types::make_shared(); - chassis->SetMass(chassis_data.mass); - chassis->SetInertiaXX(chrono::ChVector3d(0.1, 0.1, 0.1)); - chassis->SetPos(chrono::ChVector3d(chassis_data.position[0], chassis_data.position[1], chassis_data.position[2])); - chassis->SetFixed(false); - - // add visual shape for visualization - auto vis_shape = chrono_types::make_shared( - chrono::ChVector3d(chassis_data.visual_position[0], chassis_data.visual_position[1], chassis_data.visual_position[2])); - chassis -> AddVisualShape(vis_shape); - - return chassis; -} - -auto build_wheel(const utils::io::JSONFileReader& json_reader, const std::string &wheel_label) -{ - - // read the wheel daat - auto wheel_data = json_reader.template at(wheel_label); - - auto material = chrono_types::make_shared(); - - material->SetYoungModulus(2e7); // stiffness (important) - material->SetPoissonRatio(0.3); - material->SetFriction(0.9f); // traction - material->SetRestitution(0.0f); // no bouncing - - // Optional but recommended - material->SetAdhesion(0.0); - material->SetKn(2e5); // normal stiffness override - material->SetGn(40.0); // normal damping - material->SetKt(2e5); // tangential stiffness - material->SetGt(20.0); - - // rotation axis for the wheel - chrono::ChQuaternion<> q; - q.SetFromAngleAxis(chrono::CH_PI_2, chrono::ChVector3d(1, 0, 0)); - - auto wheel = chrono_types::make_shared(); - wheel->SetMass(wheel_data.mass); - wheel->SetPos(chrono::ChVector3d(wheel_data.position[0], wheel_data.position[1], wheel_data.position[2])); - wheel->SetName(wheel_label); - wheel->SetRot(q); - wheel->EnableCollision(true); - - auto cyl_shape = chrono_types::make_shared( - material, - wheel_data.radius, - wheel_data.width * 0.5 // half-length - ); - wheel->AddCollisionShape(cyl_shape); - - wheel->AddVisualShape( - chrono_types::make_shared(wheel_data.radius, wheel_data.width)); - return wheel; -} - -CHRONO_DiffDriveRobotBase::MotorHandle build_motor(std::shared_ptr wheel, - std::shared_ptr chassis, - const std::string &motor_label) -{ - - - //chrono::ChVector3d axis = chrono::VECT_Y; - - // Build joint frame in ABSOLUTE coordinates - chrono::ChQuaterniond q; - q.SetFromAngleAxis(consts::maths::PI * 0.5, chrono::VECT_X); - chrono::ChFrame<> frame(wheel->GetPos(), q); - - auto motor = chrono_types::make_shared(); - motor->Initialize( - wheel, - chassis, - frame); - motor->SetName(motor_label); - - // set the speed function (rad/s) - auto speed_func = chrono_types::make_shared(0.0); - motor -> SetSpeedFunction(speed_func); - return {motor, speed_func}; -} - - - -} - -void -CHRONO_DiffDriveRobotBase::load_from_json(const std::string& filename) -{ -#ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Loading robot from file: " << filename; -#endif - utils::io::JSONFileReader json_reader(filename); - json_reader.open(); - - auto material = chrono_types::make_shared(); - - material->SetYoungModulus(2e7); // stiffness (important) - material->SetPoissonRatio(0.3); - material->SetFriction(0.9f); // traction - material->SetRestitution(0.0f); // no bouncing - - // Optional but recommended - material->SetAdhesion(0.0); - material->SetKn(2e5); // normal stiffness override - material->SetGn(40.0); // normal damping - material->SetKt(2e5); // tangential stiffness - material->SetGt(20.0); - auto ground = chrono_types::make_shared( - 5.0, 5.0, 0.1, - 1000, - true, // visual - true, // collision - material - ); - ground->SetFixed(true); - - - // set the gravity acceleration - sys_.SetGravitationalAcceleration(chrono::ChVector3d(0, 0.0, -9.81)); - - chassis_ = build_chassis(json_reader); - sys_.Add(chassis_); - //sys_.Add(ground); - - name_ = json_reader.template get_value("name"); - - BOOST_LOG_TRIVIAL(info)<<"Building wheels...: "; - - auto left_wheel = build_wheel(json_reader, "left-wheel"); - auto right_wheel = build_wheel(json_reader, "right-wheel"); - - sys_.Add(left_wheel); - sys_.Add(right_wheel); - - auto left_motor = build_motor(left_wheel, chassis_, "left-motor"); - auto right_motor = build_motor(right_wheel, chassis_, "right-motor"); - - sys_.AddLink(left_motor.motor); - sys_.AddLink(right_motor.motor); -#ifdef BITRL_LOG - auto zleft = left_motor.motor->GetFrame2Abs().GetRot().GetAxisZ(); - BOOST_LOG_TRIVIAL(info)<<"Left motor rotation: " << zleft; - - auto zright = left_motor.motor->GetFrame2Abs().GetRot().GetAxisZ(); - BOOST_LOG_TRIVIAL(info)<<"Right motor rotation: " << zright; -#endif - - motors_ = std::make_pair(std::move(left_motor), std::move(right_motor)); - - auto caster = build_wheel(json_reader, "caster-wheel"); - sys_.Add(caster); - auto caster_joint = chrono_types::make_shared(); - caster_joint->Initialize( - caster, - chassis_, - chrono::ChFrame<>(chrono::ChCoordsys<>(caster->GetPos())) - ); - sys_.Add(caster_joint); - - // set upd the state - pose_ = std::make_shared(chassis_); - -#ifdef BITRL_LOG - BOOST_LOG_TRIVIAL(info)<<"Loaded robot: " << name_; - BOOST_LOG_TRIVIAL(info)<<"Bodies in loaded robot " << sys_.GetBodies().size(); -#endif -} - -void CHRONO_DiffDriveRobotBase::set_motor_speed(const std::string& motor_name, real_t speed) -{ - if (motor_name == "left-motor") - { - motors_.first.speed -> SetConstant(speed); - } - - if (motor_name == "right-motor") - { - motors_.second.speed -> SetConstant(speed); - } -} - -void CHRONO_DiffDriveRobotBase::step(real_t time_step) -{ - sys_.DoStepDynamics(time_step); -} -} -} -#endif - diff --git a/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h deleted file mode 100644 index 390352f..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/tmp/org_chrono_diff_drive_robot.h +++ /dev/null @@ -1,179 +0,0 @@ -#ifndef CHRONO_DIFF_DRIVE_ROBOT_H -#define CHRONO_DIFF_DRIVE_ROBOT_H - -#include "bitrl/bitrl_config.h" - -#ifdef BITRL_CHRONO - -#include "bitrl/bitrl_types.h" -#include "bitrl/sensors/sensor_manager.h" -#include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" - - -#include -#include - - -#include -#include -#include - -namespace bitrl -{ -namespace rb::bitrl_chrono -{ -/** - * @class CHRONO_DiffDriveRobotBase - * @ingroup rb_chrono - * @brief Base class for a differential-drive robot using Project Chrono. - * - * This class provides a common foundation for modeling and simulating - * differential-drive robots using Chrono physics objects. It encapsulates - * a Chrono simulation system and supports initialization from an external - * JSON configuration file. - * - * The class can be instantiated directly for simple simulations or - * extended to implement more specialized robot behaviors. - * - * @note This class assumes the use of Chrono's SMC contact model. - */ -class CHRONO_DiffDriveRobotBase -{ -public: - - typedef CHRONO_RobotPose pose_type; - - /** - * Handle the motors - */ - struct MotorHandle { - std::shared_ptr motor; - std::shared_ptr speed; - }; - - - /** - * @brief Load robot and simulation parameters from a JSON file. - * - * This function initializes the robot and its associated Chrono - * simulation objects based on the contents of the provided JSON - * configuration file. - * - * Typical parameters may include: - * - Physical dimensions - * - Mass and inertia properties - * - Wheel configuration - * - Simulation settings - * - * @param filename Path to the JSON configuration file. - * - * @throws std::runtime_error If the file cannot be read or parsed. - */ - void load_from_json(const std::string& filename); - - /** - * @brief Step the underlying chrono::ChSystemSMC one time step - * @param time_step - */ - void step(real_t time_step); - - /** - * @brief Set the motor speed - * @param motor_name The name of the motor - * @param speed The speed - */ - void set_motor_speed(const std::string& motor_name, real_t speed); - - /** - * @brief Set the speed for both motors - * @param speed - */ - void set_motor_speed(real_t speed); - - /** - * @brief The name of the robot - * @return - */ - const std::string& get_name() const noexcept{return name_;} - - /** - * @brief Retruns the number of wheels this robot has - * @return - */ - uint_t n_wheels()const noexcept{return 3;} - - /** - * @brief Returns the number of motors this robot has - * @return - */ - uint_t n_motors()const noexcept{return 2;} - - /** - * Set the pointer to the sensor manager - * @param sensors_manager - */ - void set_sensors(sensors::SensorManager& sensor_manager){sensor_manager_ptr_ = &sensor_manager;} - - /** - * @return - */ - chrono::ChSystemSMC& CHRONO_sys() noexcept{return sys_;} - - - std::shared_ptr CHRONO_chassis()noexcept{return chassis_;} - - /** - * @return Pointer to the state of the robot - */ - std::shared_ptr pose()noexcept{return pose_;} - -protected: - - - /** - * @brief Chrono physics system used for simulation. - * - * This system owns and manages all physical bodies, constraints, - * and contact interactions associated with the robot and the - * environment. - */ - chrono::ChSystemSMC sys_; - - /** - * The chassis of the robot - */ - std::shared_ptr chassis_; - - /** - * The state of the robot - */ - std::shared_ptr pose_; - - /** - * @brief Manages the sensors on the robot - */ - sensors::SensorManager* sensor_manager_ptr_; - - /** - * @brief Motors for the robot - * motors_.first left motor - * motors.second right motor - */ - std::pair motors_; - - /** - * @brief The name of the robot - */ - std::string name_; - -}; - -inline void CHRONO_DiffDriveRobotBase::set_motor_speed(real_t speed) -{ - set_motor_speed("left_motor", speed); - set_motor_speed("right_motor", speed); -} -} -} -#endif -#endif //DIFF_DRIVE_ROBOT_H diff --git a/src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h b/src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h deleted file mode 100644 index e1426e8..0000000 --- a/src/bitrl/rigid_bodies/chrono_robots/tmp/rb_chrono_module.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef RB_CHRONO_MODULE_H -#define RB_CHRONO_MODULE_H - -/** - * @defgroup rb_chrono rb::chrono - * @brief Chrono-based robotics components. - * - * This module contains robot models, simulation utilities, - * and physics-based components built on Project Chrono. - */ - -#endif //RB_CHRONO_MODULE_H diff --git a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.cpp similarity index 97% rename from src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp rename to src/bitrl/rigid_bodies/chrono_robots/turtle_robot.cpp index 0fa9200..7346e05 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp +++ b/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.cpp @@ -3,15 +3,11 @@ #ifdef BITRL_CHRONO #include "bitrl/bitrl_types.h" -#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" +#include "bitrl/rigid_bodies/chrono_robots/turtle_robot.h" -#include +#include #include -#include "chrono/physics/ChBodyEasy.h" -#include "chrono/assets/ChVisualShapeTriangleMesh.h" -#include "chrono/physics/ChMassProperties.h" - @@ -130,7 +126,7 @@ std::shared_ptr add_motor(std::shared_ptr& robot_rot, std::shared_ptr wheel_mat) @@ -158,7 +154,7 @@ CHRONO_DiffDriveRobot::CHRONO_DiffDriveRobot(chrono::ChSystem& system, -void CHRONO_DiffDriveRobot::init() { +void CHRONO_TurtleRobot::init() { // initialize robot chassis chassis_ = chrono_types::make_shared(chassis_material_, system_, robot_pos_, robot_rot_); @@ -368,7 +364,7 @@ void CHRONO_DiffDriveRobot::init() { } /// Initialize the complete rover and add all constraints -void CHRONO_DiffDriveRobot::build_motors_() { +void CHRONO_TurtleRobot::build_motors_() { // redeclare necessary location variables real_t dwx = 0; @@ -499,15 +495,15 @@ void CHRONO_DiffDriveRobot::build_motors_() { } } -void CHRONO_DiffDriveRobot::set_motor_speed(real_t rad_speed, uint_t id) { +void CHRONO_TurtleRobot::set_motor_speed(real_t rad_speed, uint_t id) { motors_func_[id]->SetConstant(rad_speed); } -chrono::ChVector3d CHRONO_DiffDriveRobot::get_active_wheel_speed(uint_t id)const { +chrono::ChVector3d CHRONO_TurtleRobot::get_active_wheel_speed(uint_t id)const { return drive_wheels_[id]->get_body_ptr()->GetPosDt(); } -chrono::ChVector3d CHRONO_DiffDriveRobot::get_active_wheel_angular_velocity(uint_t id)const { +chrono::ChVector3d CHRONO_TurtleRobot::get_active_wheel_angular_velocity(uint_t id)const { return drive_wheels_[id]->get_body_ptr()->GetAngVelParent(); } diff --git a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.h similarity index 71% rename from src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h rename to src/bitrl/rigid_bodies/chrono_robots/turtle_robot.h index 5cd698c..3493ca0 100644 --- a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h +++ b/src/bitrl/rigid_bodies/chrono_robots/turtle_robot.h @@ -1,34 +1,19 @@ - -// ============================================================================= -// Authors: Jason Zhou -// ============================================================================= -// -// Turtlebot Robot Class -// This is a modified version of the famous turtlebot 2e -// The geometries use the following resources as references: -// https://groups.google.com/g/sydney_ros/c/z05uQTCuDTQ -// https://grabcad.com/library/interbotix-turtlebot-2i-1 -// https://www.turtlebot.com/turtlebot2/ -// -// ============================================================================= - -#ifndef DIFF_DRIVE_ROBOT_H -#define DIFF_DRIVE_ROBOT_H +#ifndef TURTLE_ROBOT_H +#define TURTLE_ROBOT_H #include "bitrl/bitrl_config.h" #ifdef BITRL_CHRONO - #include "bitrl/bitrl_types.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_chassis.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_wheels.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_rods.h" -#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/diff_drive_robot_plates.h" #include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_chassis.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_plates.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_rods.h" +#include "bitrl/rigid_bodies/chrono_robots/impl/turtle_bot/turtle_robot_wheels.h" -#include "chrono/physics/ChSystem.h" -#include "chrono/physics/ChLinkMotorRotationSpeed.h" +#include +#include #include @@ -38,7 +23,7 @@ namespace rb::bitrl_chrono{ /** - * @class CHRONO_DiffDriveRobotBase + * @class CHRONO_TurtleRobot * @ingroup rb_chrono * @brief Class for modelling a differential-drive robot using Chrono. * in fact the implementation of this class is taken from the @@ -46,18 +31,18 @@ namespace rb::bitrl_chrono{ * * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html */ -class CHRONO_DiffDriveRobot final { +class CHRONO_TurtleRobot final { public: - CHRONO_DiffDriveRobot(chrono::ChSystem& system, + CHRONO_TurtleRobot(chrono::ChSystem& system, const chrono::ChVector3d& robot_pos, const chrono::ChQuaternion<>& robot_rot, std::shared_ptr wheel_mat = {}); /// destructor - ~CHRONO_DiffDriveRobot()=default; + ~CHRONO_TurtleRobot()=default; - /// Initialize the CHRONO_DiffDriveRobot robot + /// Initialize the CHRONO_TurtleRobot robot /// The robot will not be constructed in the ChSystem until init() is called. void init(); diff --git a/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp index 0bfc77f..24b6672 100644 --- a/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.cpp @@ -5,9 +5,8 @@ #include "bitrl/utils/io/json_file_reader.h" #include "bitrl/sensors/backends/chrono_ultrasound_backend.h" -#include "chrono/collision/ChCollisionSystem.h" - +#include #ifdef BITRL_LOG #include @@ -20,15 +19,27 @@ namespace bitrl namespace sensors::backends { +// namespace{ +// using json = nlohmann::json; +// } + const std::string CHRONO_UltrasonicBackend::BACKEND_TYPE = "CHRONO_UltrasonicBackend"; +CHRONO_UltrasonicBackend::CHRONO_UltrasonicBackend() + : +RangeSensorBackendBase(CHRONO_UltrasonicBackend::BACKEND_TYPE), +sys_ptr_(nullptr) +{} + + CHRONO_UltrasonicBackend::CHRONO_UltrasonicBackend(chrono::ChSystem& sys_ptr, std::shared_ptr body) : RangeSensorBackendBase(CHRONO_UltrasonicBackend::BACKEND_TYPE), -sys_ptr_(&sys_ptr), -body_(body) -{} +sys_ptr_(&sys_ptr) +{ + this -> set_chrono_body(body); +} void CHRONO_UltrasonicBackend::load_from_json(const std::string& filename) { @@ -55,8 +66,6 @@ void CHRONO_UltrasonicBackend::load_from_json(const std::string& filename) for (size_t i = 0; i < 3; ++i) position_[i] = pos.at(i).get(); - - #ifdef BITRL_LOG BOOST_LOG_TRIVIAL(info)<<"Loaded sensor backend: CHRONO_UltrasonicBackend"; #endif @@ -93,6 +102,9 @@ std::vector CHRONO_UltrasonicBackend::read_values() return {this -> max_distance()}; } + + + } } diff --git a/src/bitrl/sensors/backends/chrono_ultrasound_backend.h b/src/bitrl/sensors/backends/chrono_ultrasound_backend.h index 6c18f36..e5f78d3 100644 --- a/src/bitrl/sensors/backends/chrono_ultrasound_backend.h +++ b/src/bitrl/sensors/backends/chrono_ultrasound_backend.h @@ -7,11 +7,13 @@ #include "bitrl/sensors/backends/range_sensor_backend_base.h" -#include "chrono/physics/ChSystem.h" -#include "chrono/physics/ChBody.h" +#include +#include #include #include +#include +#include namespace bitrl { @@ -32,9 +34,14 @@ class CHRONO_UltrasonicBackend final: public RangeSensorBackendBase */ static const std::string BACKEND_TYPE; - /** - * @brief Constructor - */ + /// + /// @brief Constructor + /// + CHRONO_UltrasonicBackend(); + + /// + /// @brief Constructor + /// CHRONO_UltrasonicBackend(chrono::ChSystem& sys_ptr, std::shared_ptr body); @@ -63,6 +70,7 @@ class CHRONO_UltrasonicBackend final: public RangeSensorBackendBase */ void load_from_json(const std::string& filename); + /** * @brief Set the position of the sensor * @param pos @@ -80,12 +88,7 @@ class CHRONO_UltrasonicBackend final: public RangeSensorBackendBase /** * @brief Pointer to the system this sensor is attached ot */ - chrono::ChSystem* sys_ptr_; - - /** - * @brief The body on which the sensor sits - */ - std::shared_ptr body_; + chrono::ChSystem* sys_ptr_{nullptr}; /** * @brief The position of the sensor diff --git a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp index a57f250..813ce10 100644 --- a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp +++ b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.cpp @@ -14,6 +14,12 @@ namespace sensors::backends { const std::string MQTT_UltrasonicBackend::BACKEND_TYPE="MQTT_UltrasonicBackend"; +MQTT_UltrasonicBackend::MQTT_UltrasonicBackend() + : +RangeSensorBackendBase(MQTT_UltrasonicBackend::BACKEND_TYPE), +subscriber_ptr_(nullptr) +{} + MQTT_UltrasonicBackend::MQTT_UltrasonicBackend(network::MqttSubscriber& subscriber) : diff --git a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h index 1514507..d9bacfb 100644 --- a/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h +++ b/src/bitrl/sensors/backends/mqtt_ultrasound_backend.h @@ -10,6 +10,8 @@ #include "bitrl/sensors/backends/range_sensor_backend_base.h" #include "bitrl/sensors/messages/ultrasound.h" + + namespace bitrl{ namespace sensors::backends { @@ -30,9 +32,14 @@ class MQTT_UltrasonicBackend: public RangeSensorBackendBase */ static const std::string BACKEND_TYPE; - /** - * @brief Constructor - */ + /// + /// @brief Constructor + /// + MQTT_UltrasonicBackend(); + + /// + /// @brief Constructor + /// explicit MQTT_UltrasonicBackend(network::MqttSubscriber& subscriber); /** diff --git a/src/bitrl/sensors/backends/range_sensor_backend_base.h b/src/bitrl/sensors/backends/range_sensor_backend_base.h index 6b06261..285e6ed 100644 --- a/src/bitrl/sensors/backends/range_sensor_backend_base.h +++ b/src/bitrl/sensors/backends/range_sensor_backend_base.h @@ -1,10 +1,16 @@ #ifndef RANGE_SENSOR_BACKEND_BASE_H #define RANGE_SENSOR_BACKEND_BASE_H - +#include "bitrl/bitrl_config.h" #include "bitrl/bitrl_types.h" #include "bitrl/sensors/backends/sensor_backend_base.h" #include +#include + +#ifdef BITRL_CHRONO +#include +#include +#endif namespace bitrl { @@ -18,6 +24,10 @@ namespace sensors::backends class RangeSensorBackendBase : public SensorBackendBase { public: + + + static std::shared_ptr create(const std::string& backend_type); + /** * @return The maximum distance the range sensor can read */ @@ -52,6 +62,15 @@ class RangeSensorBackendBase : public SensorBackendBase */ void set_sensor_units(const std::string& units){sensor_units_ = units;} +#ifdef BITRL_CHRONO + /// + /// @brief Add a visual shape for the sensor in order to + /// be visualized with Irrlicht. Concrete classes may choose not + /// to implement this. In this case the sensor will not be visible + /// + virtual void add_visual_shape(const std::unordered_map& shape_properties)override; +#endif + protected: /** * @brief Constructor diff --git a/src/bitrl/sensors/backends/sensor_backend_base.h b/src/bitrl/sensors/backends/sensor_backend_base.h index 46198ee..36014ad 100644 --- a/src/bitrl/sensors/backends/sensor_backend_base.h +++ b/src/bitrl/sensors/backends/sensor_backend_base.h @@ -3,6 +3,14 @@ #include "bitrl/bitrl_types.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO +#include +#include +#include +#endif + #include #include @@ -80,6 +88,19 @@ class SensorBackendBase */ std::string backend_type_str()const noexcept{return backend_type_;} +#ifdef BITRL_CHRONO + /// + /// @brief Add a visual shape for the sensor in order to + /// be visualized with Irrlicht. Concrete classes may choose not + /// to implement this. In this case the sensor will not be visible + /// + virtual void add_visual_shape(const std::unordered_map& shape_properties)=0; + + /// @brief Set the pointer to the body that this sensor is attached + /// @param body + void set_chrono_body(std::shared_ptr body){body_ = body;} +#endif + protected: /** * @brief Constructor @@ -87,6 +108,13 @@ class SensorBackendBase */ SensorBackendBase(const std::string& backend_type); +#ifdef BITRL_CHRONO + /// + /// @brief The body on which the sensor sits + /// + std::shared_ptr body_{nullptr}; +#endif + private: /** diff --git a/src/bitrl/sensors/sensor_base.h b/src/bitrl/sensors/sensor_base.h index 7083d5b..d4bfab6 100644 --- a/src/bitrl/sensors/sensor_base.h +++ b/src/bitrl/sensors/sensor_base.h @@ -5,6 +5,8 @@ #include "../bitrl_consts.h" #include "bitrl/bitrl_types.h" #include "bitrl/bitrl_consts.h" +#include "bitrl/sensors/backends/sensor_backend_base.h" + #include #include namespace bitrl @@ -92,6 +94,8 @@ class SensorBase */ std::string sensor_type()const noexcept{return sensor_type_;} + virtual std::shared_ptr backend_ptr()const{return backend_ptr_;} + protected: /** * @param sensor_type The type of the sensor @@ -104,6 +108,11 @@ class SensorBase */ std::vector values_; + /// + /// @brief Pointer to backend tha actually handles the sensor + /// + std::shared_ptr backend_ptr_; + private: /** diff --git a/src/bitrl/sensors/sensor_manager.h b/src/bitrl/sensors/sensor_manager.h index 8aefa5b..a6f6ada 100644 --- a/src/bitrl/sensors/sensor_manager.h +++ b/src/bitrl/sensors/sensor_manager.h @@ -22,6 +22,9 @@ class SensorManager: private boost::noncopyable { public: + typedef std::vector< std::shared_ptr>::iterator iterator; + typedef std::vector< std::shared_ptr >::const_iterator const_iterator; + explicit SensorManager(uint_t n_sensors); /** @@ -40,6 +43,12 @@ class SensorManager: private boost::noncopyable */ uint_t n_sensors() const { return sensors_.size(); } + iterator begin() { return sensors_.begin(); } + iterator end() { return sensors_.end(); } + + const_iterator begin() const { return sensors_.begin(); } + const_iterator end() const { return sensors_.end(); } + private: uint_t n_sensors_; diff --git a/src/bitrl/sensors/sensor_type_enum.h b/src/bitrl/sensors/sensor_type_enum.h index 1a0d599..74a2715 100644 --- a/src/bitrl/sensors/sensor_type_enum.h +++ b/src/bitrl/sensors/sensor_type_enum.h @@ -15,7 +15,7 @@ enum class SensorTypeEnum : uint_t INVALID_TYPE = 0, CAMERA = 1, ULTRASOUND = 2, - ODMETRY = 3, + ODOMETRY = 3, VECTOR_SENSOR = 4 }; } diff --git a/src/bitrl/sensors/ultrasonic_sensor.cpp b/src/bitrl/sensors/ultrasonic_sensor.cpp index b8e066e..e1d8a7f 100644 --- a/src/bitrl/sensors/ultrasonic_sensor.cpp +++ b/src/bitrl/sensors/ultrasonic_sensor.cpp @@ -1,5 +1,6 @@ #include "bitrl/bitrl_config.h" #include "bitrl/sensors/ultrasonic_sensor.h" +#include "bitrl/sensors/backends/range_sensor_backend_base.h" #ifdef BITRL_LOG #include @@ -13,6 +14,12 @@ namespace sensors { const std::string UltrasonicSensor::SENSOR_TYPE = "UltrasonicSensor"; +std::shared_ptr +UltrasonicSensor::create(const std::string& backend_type, const std::string& name) +{ + auto backend = sensors::backends::RangeSensorBackendBase::create(backend_type); + return std::make_shared(backend, name); +} void UltrasonicSensor::init() { diff --git a/src/bitrl/sensors/ultrasonic_sensor.h b/src/bitrl/sensors/ultrasonic_sensor.h index 6ab001d..49f8144 100644 --- a/src/bitrl/sensors/ultrasonic_sensor.h +++ b/src/bitrl/sensors/ultrasonic_sensor.h @@ -26,6 +26,12 @@ class UltrasonicSensor final: public SensorBase */ static const std::string SENSOR_TYPE; + /// + /// @param backend_type + /// @param name + /// @return + static std::shared_ptr create(const std::string& backend_type, const std::string& name); + /** * @brief Constructor * @param backend @@ -57,6 +63,9 @@ class UltrasonicSensor final: public SensorBase */ virtual std::string sensor_units()const; + + virtual std::shared_ptr backend_ptr()const override{return backend_;} + private: /** * @brief The backend used for this sensor diff --git a/src/bitrl/utils/io/csv_file_reader.cpp b/src/bitrl/utils/io/csv_file_reader.cpp index b0007c4..42ffac9 100644 --- a/src/bitrl/utils/io/csv_file_reader.cpp +++ b/src/bitrl/utils/io/csv_file_reader.cpp @@ -9,11 +9,11 @@ #include #endif +#include + namespace bitrl { -namespace utils -{ -namespace io +namespace utils::io { CSVFileReader::CSVFileReader(const std::string &file_name, const std::string delimeter) @@ -71,6 +71,5 @@ std::vector CSVFileReader::read_line_as_uint() return line_int; } -} // namespace io -} // namespace utils +} // namespace utils::io } // namespace bitrl diff --git a/src/bitrl/utils/io/csv_file_reader.h b/src/bitrl/utils/io/csv_file_reader.h index 8b99234..fd18b7f 100644 --- a/src/bitrl/utils/io/csv_file_reader.h +++ b/src/bitrl/utils/io/csv_file_reader.h @@ -4,8 +4,6 @@ #include "bitrl/bitrl_types.h" #include "bitrl/utils/io/file_reader_base.h" -#include -#include #include #include diff --git a/src/bitrl/utils/io/file_handler_base.h b/src/bitrl/utils/io/file_handler_base.h index ad0c90e..6e1fc81 100644 --- a/src/bitrl/utils/io/file_handler_base.h +++ b/src/bitrl/utils/io/file_handler_base.h @@ -1,6 +1,3 @@ -// SPDX-FileCopyrightText: 2024 -// SPDX-License-Identifier: Apache-2.0 - #ifndef FILE_HANDLER_BASE_H #define FILE_HANDLER_BASE_H @@ -8,15 +5,56 @@ #include "boost/noncopyable.hpp" #include +#include namespace bitrl { -namespace utils -{ -namespace io +namespace utils::io { /** - * @todo write docs + * @class FileHandlerBase + * @brief Abstract base class for file handling operations. + * + * This class provides a common interface and shared functionality for + * managing file input/output operations. It encapsulates a low-level + * file handler (e.g., std::ifstream, std::ofstream, or similar types) + * and enforces a consistent API for derived classes that implement + * specific file reading or writing behavior. + * + * The class is non-copyable (via boost::noncopyable) to prevent + * unintended duplication of file handles. + * + * @tparam HandlerType The underlying file stream/handler type. + * + * ### Responsibilities + * - Store and expose the file name and file format. + * - Manage the lifecycle of the file stream (open/close). + * - Provide access to the underlying file handler. + * - Ensure proper cleanup by closing the file in the destructor. + * + * ### Usage + * This class is intended to be inherited by concrete file handler + * implementations (e.g., readers or writers). Derived classes must + * implement the open() method to define how the file is opened. + * + * ### Example + * @code + * class MyFileReader : public FileHandlerBase { + * public: + * using FileHandlerBase::FileHandlerBase; + * + * void open() override { + * f_.open(file_name_, std::ios::in); + * } + * }; + * @endcode + * + * ### Notes + * - The destructor automatically calls close() to ensure the file + * is properly released. + * - Calling close() on an already closed file is safe. + * - The class does not handle error reporting; this should be + * implemented in derived classes if needed. */ template class FileHandlerBase : private boost::noncopyable { @@ -24,69 +62,117 @@ template class FileHandlerBase : private boost::noncopyab typedef HandlerType handler_type; /// - /// \brief Constructor + /// @brief Destructor. Ensures that the file is properly closed. + /// + /// @post The underlying file stream is closed. /// virtual ~FileHandlerBase(); /// - /// \brief Returns the type of the file + /// @brief Returns the type/format of the file. + /// + /// @return The file format as a FileFormats::Type value. + /// @post Does not modify object state. /// FileFormats::Type get_type() const noexcept { return t_; } /// - /// \brief Returns the underlying file stream + /// @brief Provides mutable access to the underlying file stream. + /// + /// @return Reference to the underlying file handler. + /// @pre The file may or may not be open. /// handler_type &get_file_stream() noexcept { return f_; } /// - /// \brief Returns the underlying file stream + /// @brief Provides read-only access to the underlying file stream. + /// + /// @return Const reference to the underlying file handler. /// const handler_type &get_file_stream() const noexcept { return f_; } /// - /// \brief Returns the filename that is used to write + /// @brief Returns the file name associated with this handler. + /// + /// @return The file name as a std::string. /// std::string get_filename() const noexcept { return file_name_; } - /** - * \brief Return true if and only if the file is open - * - */ + /// + /// @brief Returns the file path associated with this handler. + /// + /// @return The file name as a std::filesystem::path object. + /// + std::filesystem::path get_file_path() const noexcept { return file_name_; } + + /// + /// @brief Checks whether the file is currently open. + /// + /// @return true if the file stream is open, false otherwise. + /// bool is_open() const noexcept { return f_.is_open(); } - /** - * \brief Close the file. Return true if and only if the file - * was closed successfully false otherwise - * - */ + /// + /// @brief Closes the file if it is currently open. + /// + /// If the underlying file stream is open, this function attempts + /// to close it. If the file is already closed, no action is taken. + /// + /// @post The file stream is not open (is_open() == false). + /// \throws May propagate exceptions thrown by handler_type::close() + /// if the underlying stream supports throwing exceptions. + /// \note Calling this function multiple times is safe. + /// virtual void close(); - /** - * @brief Open the file - */ + /// + /// @brief Opens the file. + /// + /// This is a pure virtual function that must be implemented by + /// derived classes. Implementations should define how the file + /// is opened (e.g., read/write mode, binary/text mode, etc.). + /// + /// @pre The file is not already open, unless the derived class + /// explicitly supports reopening. + /// @post If successful, is_open() == true. + /// @throws Implementation-defined. Derived classes should document + /// any exceptions or error-handling behavior. + /// virtual void open() = 0; protected: /** - * @brief protected Constructor so that explicit instantiation of - * the class fails + * @brief Protected constructor. + * + * Initializes the file handler with a file name and format. + * + * @param file_name The name (or path) of the file to be handled. + * @param t The file format/type. + * + * @pre file_name should not be empty. + * @post The handler is initialized but the file is not open. */ FileHandlerBase(const std::string &file_name, FileFormats::Type t); /** - * @brief The name of the file to write - * - */ - + * @brief The name of the file to read from or write to. + * + * Represents the path used when opening the file. + */ std::string file_name_; /// - /// \brief The format of the file + /// \brief The format/type of the file. + /// + /// Immutable after construction. /// const FileFormats::Type t_; /** - * @brief The low level file handler + * @brief The underlying file stream/handler. + * + * This object represents the low-level file interface + * (e.g., std::ifstream, std::ofstream, etc.). */ handler_type f_; }; @@ -106,8 +192,7 @@ template void FileHandlerBase::close() f_.close(); } -} // namespace io -} // namespace utils +} // namespace utils::io } // namespace bitrl #endif // FILE_HANDLER_BASE_H diff --git a/src/bitrl/utils/io/file_reader_base.cpp b/src/bitrl/utils/io/file_reader_base.cpp index 75a8b55..21d1a5f 100644 --- a/src/bitrl/utils/io/file_reader_base.cpp +++ b/src/bitrl/utils/io/file_reader_base.cpp @@ -1,6 +1,10 @@ #include "bitrl/utils/io/file_reader_base.h" #include "bitrl/bitrl_config.h" +#ifdef BITRL_LOG +#include +#endif + namespace bitrl { namespace utils::io @@ -14,6 +18,12 @@ FileReaderBase::FileReaderBase(const std::string &file_name, FileFormats::Type t void FileReaderBase::open() { + auto file_name = this->get_filename(); + +#ifdef BITRL_LOG + BOOST_LOG_TRIVIAL(info)<<"Reading file: "<get_file_stream(); if (!f.is_open()) @@ -21,13 +31,12 @@ void FileReaderBase::open() try { - f.open(this->get_filename(), std::ios_base::in); + f.open(file_name, std::ios_base::in); #ifdef BITRL_DEBUG if (!f.good()) { - auto file_name = this->get_filename(); std::string msg = "Failed to open file: " + file_name; assert(false && msg.c_str()); } @@ -38,7 +47,6 @@ void FileReaderBase::open() #ifdef BITRL_DEBUG std::string msg("Failed to open file: "); - auto file_name = this->get_filename(); msg += file_name; assert(false && msg.c_str()); #else diff --git a/src/bitrl/utils/io/json_file_reader.cpp b/src/bitrl/utils/io/json_file_reader.cpp index 5df9804..feb21e9 100644 --- a/src/bitrl/utils/io/json_file_reader.cpp +++ b/src/bitrl/utils/io/json_file_reader.cpp @@ -1,6 +1,3 @@ -// SPDX-FileCopyrightText: 2024 -// SPDX-License-Identifier: Apache-2.0 - #include "bitrl/utils/io/json_file_reader.h" #include "bitrl/utils/io/file_formats.h" @@ -14,9 +11,13 @@ JSONFileReader::JSONFileReader(const std::string &filename) { } +JSONFileReader::JSONFileReader(const std::filesystem::path &filename) + : +FileReaderBase(filename.string(), FileFormats::Type::JSON), data_() +{} + void JSONFileReader::open() { - this->FileReaderBase::open(); auto &f = this->get_file_stream(); data_ = json::parse(f); diff --git a/src/bitrl/utils/io/json_file_reader.h b/src/bitrl/utils/io/json_file_reader.h index 8668795..99ac4ae 100644 --- a/src/bitrl/utils/io/json_file_reader.h +++ b/src/bitrl/utils/io/json_file_reader.h @@ -3,6 +3,7 @@ #include "bitrl/extern/nlohmann/json/json.hpp" #include "bitrl/utils/io/file_reader_base.h" +#include namespace bitrl { @@ -24,6 +25,10 @@ class JSONFileReader final : public FileReaderBase */ JSONFileReader(const std::string &filename); + /// Read the JSON file from the given filename + /// @param filename The std::filesystem::path to read the file from + JSONFileReader(const std::filesystem::path &filename); + /** * @brief Attempts to open the file for reading */ From 11844c2856d02707efc547786b822d664d8065bf Mon Sep 17 00:00:00 2001 From: pockerman Date: Sun, 29 Mar 2026 19:45:48 +0100 Subject: [PATCH 2/2] Add diff drive robot class (#179) --- .../chrono_robots/diff_drive_robot.cpp | 407 ++++++++++++++++++ .../chrono_robots/diff_drive_robot.h | 162 +++++++ .../backends/range_sensor_backend_base.cpp | 52 +++ 3 files changed, 621 insertions(+) create mode 100644 src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp create mode 100644 src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h create mode 100644 src/bitrl/sensors/backends/range_sensor_backend_base.cpp diff --git a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp new file mode 100644 index 0000000..f0ce67a --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.cpp @@ -0,0 +1,407 @@ +#include "bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h" + + +#ifdef BITRL_CHRONO + +#include "bitrl/utils/io/json_file_reader.h" +#include "bitrl/extern/nlohmann/json/json.hpp" +#include "bitrl/sensors/ultrasonic_sensor.h" +#include + +#include +#include +#include +#include +#include +#include + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ +namespace +{ + +using json = nlohmann::json; + +// helper structs to read data from JSON +struct MaterialData +{ + real_t friction; + real_t restitution; + MaterialData(const json& j) + : + friction(j.at("friction").get()), + restitution(j.at("restitution").get()) + {} +}; + +struct ChassisData +{ + real_t density{0.0}; + real_t wheel_radius{0.0}; + real_t wheel_width{0.0}; + real_t chassis_x; + real_t chassis_y; + real_t chassis_z; + real_t pos_x; + real_t pos_y; + real_t pos_z; + + ChassisData(const json& j) + : + chassis_x(j.at("chassis_x").get()), + chassis_y(j.at("chassis_y").get()), + chassis_z(j.at("chassis_z").get()), + pos_x(j.at("pos_x").get()), + pos_y(j.at("pos_y").get()), + pos_z(j.at("pos_z").get()) + {} + +}; + +struct WheelData +{ + real_t density; + real_t wheel_radius; + real_t wheel_width; + real_t pos_x; + real_t pos_y; + real_t pos_z; + + WheelData(const json& j) + : + pos_x(j.at("pos_x").get()), + pos_y(j.at("pos_y").get()), + pos_z(j.at("pos_z").get()) + {} +}; + +struct CasterWheelData +{ + real_t density; + real_t wheel_radius; + real_t pos_x; + real_t pos_y; + real_t pos_z; + CasterWheelData(const json& j) + : + pos_x(j.at("pos_x").get()), + pos_y(j.at("pos_y").get()), + pos_z(j.at("pos_z").get()) + {} +}; + +// helper functions to build the various entities of the robot +auto build_material(const MaterialData& md) +{ + auto material = chrono_types::make_shared(); + material->SetFriction(md.friction); + material->SetRestitution(md.restitution); + return material; +} + +auto build_chassis(std::shared_ptr material, const ChassisData& cd) +{ + auto chassis = chrono_types::make_shared(cd.chassis_x, cd.chassis_y, cd.chassis_z, + cd.density, true, true, material); + + chrono::ChVector3d pos(cd.pos_x, cd.pos_y, cd.wheel_radius + cd.chassis_z/2.0 + 0.01); + chassis->SetPos(pos); + chassis->EnableCollision(true); + return chassis; +} + +auto build_wheel(std::shared_ptr material, const WheelData& wd) +{ + auto wheel = chrono_types::make_shared(chrono::ChAxis::Y, + wd.wheel_radius,wd.wheel_width, + wd.density, true, true, material); + + const chrono::ChVector3d pos(wd.pos_x, wd.pos_y, wd.pos_z); + wheel->SetPos(pos); + wheel->EnableCollision(true); + return wheel; +} + +auto build_caster_wheel(std::shared_ptr material, + const CasterWheelData& cwd, + bool visualization=true, + bool collision=true + ) +{ + + auto caster = chrono_types::make_shared( + cwd.wheel_radius, // radius + cwd.density, // density + visualization, // visualization + collision, // collision + material); + + const chrono::ChVector3d pos(cwd.pos_x, cwd.pos_y, cwd.pos_z); + caster->SetPos(pos); + caster->EnableCollision(true); + return caster; +} + +auto build_caster_joint(std::shared_ptr caster, + std::shared_ptr chassis, + chrono::ChVector3d& pos) +{ + auto caster_joint = chrono_types::make_shared(); + caster_joint->Initialize( + caster, + chassis, + chrono::ChFrame<>(pos, chrono::QUNIT)); + return caster_joint; + +} + +auto build_motor(std::shared_ptr wheel, + std::shared_ptr speed_func, + std::shared_ptr chassis, + const chrono::ChVector3d& pos) +{ + auto motor = chrono_types::make_shared(); + chrono::ChQuaternion<> rot = chrono::QuatFromAngleX(chrono::CH_PI_2); + motor->Initialize( + wheel, + chassis, + chrono::ChFrame<>(pos, rot) + ); + + motor->SetSpeedFunction(speed_func); + return motor; +} + +} + +template<> +void CHRONO_DiffDriveRobot::RobotData::read(const json& j) +{ + max_speed = j.at("max_speed").get(); + wheel_radius = j.at("wheel_radius").get(); + wheel_width = j.at("wheel_width").get(); + axle_length = j.at("axle_length").get(); + density = j.at("density").get(); + caster_wheel_radius = j.at("caster_wheel_radius").get(); +} + +namespace +{ + +using json = nlohmann::json; + +struct RangeSensorData +{ + uint_t idx; + bool enable; + real_t update_rate; + real_t max_distance; + real_t min_distance; + std::string backend; + std::string name; + std::vector color; + std::vector frame_pos; + std::vector dimensions; + + RangeSensorData(const json& data) + : + idx(data.at("idx").get()), + enable(data.at("enable").get()), + update_rate(data.at("update_rate").get()), + max_distance(data.at("max_distance").get()), + min_distance(data.at("min_distance").get()), + backend(data.at("backend").get()), + name(data.at("name").get()), + color(data.at("color").get>()), + frame_pos(data.at("frame_pos").get>()), + dimensions(data.at("dimensions").get>()) + {} + +}; +} + +CHRONO_DiffDriveRobot::RobotSensorManager::RobotSensorManager() + : +sensor_manager_(0) +{} + +template<> +void CHRONO_DiffDriveRobot::RobotSensorManager::read(const json& j, std::shared_ptr body) +{ + + auto sensors = j.at("sensors"); + auto fsdata = RangeSensorData(sensors.at("front-sonar")); + + auto fs = sensors::UltrasonicSensor::create(fsdata.backend, fsdata.name); + sensor_manager_.add(fs); + + // get the backend and create the visual shape + auto backend_ptr = fs -> backend_ptr(); + + if (!backend_ptr) + { + throw std::runtime_error("Backend ptr pointer is not set"); + } + + backend_ptr -> set_chrono_body(body); + chrono::ChFrame<> sensor_frame( + chrono::ChVector3d(fsdata.frame_pos[0], fsdata.frame_pos[1], fsdata.frame_pos[1]) + ); + + chrono::ChColor color(fsdata.color[0], fsdata.color[1], fsdata.color[2]); + std::unordered_map vis_shape_properties; + vis_shape_properties["chrono::ChColor"] = color; + vis_shape_properties["chrono::ChFrame<>"] = sensor_frame; + vis_shape_properties["x_length"] = fsdata.dimensions[0]; + vis_shape_properties["y_length"] = fsdata.dimensions[1]; + vis_shape_properties["z_length"] = fsdata.dimensions[2]; + backend_ptr -> add_visual_shape(vis_shape_properties); +} + +void CHRONO_DiffDriveRobot::add_to_sys(chrono::ChSystemNSC& sys) const +{ + if (!is_loaded_) + { + throw std::logic_error("Robot has not been loaded have you called load?"); + } + + sys.Add(chassis_); + sys.Add(left_wheel_); + sys.Add(right_wheel_); + sys.Add(left_wheel_motor_); + sys.Add(right_wheel_motor_); + sys.Add(caster_wheel_); + sys.Add(caster_link_joint_); +} + +void CHRONO_DiffDriveRobot::set_speed(real_t speed) +{ + if (!is_loaded_) + { + throw std::logic_error("Robot has not been loaded have you called load?"); + } + + // find out the current velocity vector + auto current_vel_vec = chassis_ -> GetPosDt(); + chrono::ChVector3d vel(speed, current_vel_vec[1], current_vel_vec[2]); + chassis_ -> SetPosDt(vel); + auto speed_ = vel.Length(); + + //set the motors speed + auto omega = speed_ / data_.wheel_radius; + set_motor_speed(omega); + +} + +void CHRONO_DiffDriveRobot::load(const std::filesystem::path& filename) +{ + + if (is_loaded_) + { + return; + } + + utils::io::JSONFileReader reader(filename); + reader.open(); + + data_.read(reader.get("robot-data")); + + auto md = reader.at("material"); + auto material = build_material(md); + + auto cd = reader.at("chassis"); + cd.wheel_radius = data_.wheel_radius; + cd.wheel_width = data_.wheel_width; + cd.density = data_.density; + chassis_ = build_chassis(material, cd); + + auto lwd = reader.at("left-wheel"); + lwd.wheel_radius = data_.wheel_radius; + lwd.wheel_width = data_.wheel_width; + lwd.density = data_.density; + left_wheel_ = build_wheel(material, lwd); + + auto rwd = reader.at("right-wheel"); + rwd.wheel_radius = data_.wheel_radius; + rwd.wheel_width = data_.wheel_width; + rwd.density = data_.density; + right_wheel_ = build_wheel(material, rwd); + + auto cwd = reader.at("caster-wheel"); + cwd.wheel_radius = data_.caster_wheel_radius; + cwd.density = data_.density; + caster_wheel_ = build_caster_wheel(material, cwd); + + chrono::ChVector3d pos(-0.25, 0, cwd.wheel_radius); + caster_link_joint_ = build_caster_joint(caster_wheel_, chassis_, pos); + + left_wheel_motor_speed_func_ = chrono_types::make_shared(0.0); + left_wheel_motor_ = build_motor(left_wheel_, + left_wheel_motor_speed_func_, + chassis_, chrono::ChVector3d(0,0.2,0.1)); + + right_wheel_motor_speed_func_ = chrono_types::make_shared(0.0); + right_wheel_motor_ = build_motor(right_wheel_, right_wheel_motor_speed_func_, + chassis_, chrono::ChVector3d(0.0,-0.2,0.1)); + + // track the chassis for pose reports + pose_.set_body(chassis_); + + // read the sensors + sensor_manager_.read(reader.get("sensors"), chassis_); + + // set the loaded flag to true to signal + // successful loading + is_loaded_ = true; +} + +void CHRONO_DiffDriveRobot::set_motor_speed(uint_t m, real_t speed) +{ + if (!is_loaded_) + { + throw std::logic_error("Robot has not been loaded have you called load?"); + } + + if (m !=0 || m != 1) + { + throw std::logic_error("Invalid motor index. Index must be either 0 or 1"); + } + + if (m == 0) + { + left_wheel_motor_speed_func_ -> SetConstant(speed); + } + + if (m == 1) + { + right_wheel_motor_speed_func_ -> SetConstant(speed); + } + +} + +void CHRONO_DiffDriveRobot::set_motor_speed(real_t speed) +{ + set_motor_speed(0, speed); + set_motor_speed(1, speed); + + // now calculate the linear velocities of the motors + auto vl = speed * data_.wheel_radius; + auto vr = speed * data_.wheel_radius; + + auto v = 0.5 * (vl + vr); + auto omega = (vr - vl) * data_.axle_length; + + auto vx = v * std::cos(omega); + auto vy = v * std::sin(omega); + + chrono::ChVector3d vel(vx, vy, 0.0); + chassis_ -> SetPosDt(vel); +} + +} +} + + +#endif diff --git a/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h new file mode 100644 index 0000000..a77e9a0 --- /dev/null +++ b/src/bitrl/rigid_bodies/chrono_robots/diff_drive_robot.h @@ -0,0 +1,162 @@ +#ifndef DIFF_DRIVE_ROBOT_H +#define DIFF_DRIVE_ROBOT_H + +#include "bitrl/bitrl_config.h" + +#ifdef BITRL_CHRONO + +#include "bitrl/bitrl_types.h" +#include "bitrl/rigid_bodies/chrono_robots/chrono_robot_pose.h" +#include "bitrl/sensors/sensor_manager.h" +#include "bitrl/sensors/sensor_base.h" + +#include +#include +#include + +#include +#include + +namespace bitrl +{ +namespace rb::bitrl_chrono +{ + +/** + * @class CHRONO_TurtleRobot + * @ingroup rb_chrono + * @brief Class for modelling a differential-drive robot using Chrono. + * in fact the implementation of this class is taken from the + * Chrono::TurtleBot. + * + * For more details see here: https://api.projectchrono.org/group__robot__models__turtlebot.html + */ +class CHRONO_DiffDriveRobot +{ +public: + + /// + /// Helper class to assemble some robot data + /// + struct RobotData + { + real_t max_speed; + real_t wheel_radius; + real_t wheel_width; + real_t axle_length; + real_t density; + real_t caster_wheel_radius; + + template + void read(const Archive& arch); + }; + + /// + /// Class to manage the sensors attached on this + /// + class RobotSensorManager + { + + public: + + /// + /// Constructor + /// + RobotSensorManager(); + + /// Load the sensors of the robot from the archive + /// @tparam Archive + /// @param arch + template + void read(const Archive& arch, std::shared_ptr body); + + /// + /// @return The number of registered senosrs + uint_t n_sensors()const noexcept{return sensor_manager_.n_sensors();} + + /// + /// @brief Add a new sensor to manage + /// @param sensor + /// + void add(std::shared_ptr sensor){sensor_manager_.add(sensor);} + + private: + + bitrl::sensors::SensorManager sensor_manager_; + + }; + + /// + /// Constructor + /// + CHRONO_DiffDriveRobot()=default; + + /// Load the robot details form the specified filename + void load(const std::filesystem::path& filename); + + /// Brief return the number of wheel motors this robot has + /// @return + /// + uint_t n_wheel_motors()const noexcept{return 2;} + + /// + /// Set the speed of the robot. + /// This function sets the x-component of the velocity vector + /// of the chassis + /// @param speed + void set_speed(real_t speed); + + /// + /// Set the speed of the m-th motor + /// m=0 => left motor + /// m=1 => right motor + void set_motor_speed(uint_t m, real_t speed); + + /// + /// Set both motors to the same speed + /// @param speed + /// + void set_motor_speed(real_t speed); + + /// + /// Get the pose of the robot + /// + const CHRONO_RobotPose& get_pose() const{return pose_; } + + /// + /// Add the components of this robot to the given chrono::ChSystemNSC + /// @param sys + void add_to_sys(chrono::ChSystemNSC& sys) const; + + /// Read/write reference to the senosr manager for this robot + /// @return + /// + RobotSensorManager & get_sensor_manager(){return sensor_manager_;} + + +private: + + /// the components of the robot + std::shared_ptr chassis_; + std::shared_ptr left_wheel_; + std::shared_ptr right_wheel_; + std::shared_ptr left_wheel_motor_; + std::shared_ptr left_wheel_motor_speed_func_; + std::shared_ptr right_wheel_motor_; + std::shared_ptr right_wheel_motor_speed_func_; + std::shared_ptr caster_wheel_; + std::shared_ptr caster_link_joint_; + + /// The pose of the chassis + CHRONO_RobotPose pose_; + RobotData data_; + RobotSensorManager sensor_manager_; + + bool is_loaded_{false}; +}; +} +} + +#endif + +#endif //DIFF_DRIVE_ROBOT_H diff --git a/src/bitrl/sensors/backends/range_sensor_backend_base.cpp b/src/bitrl/sensors/backends/range_sensor_backend_base.cpp new file mode 100644 index 0000000..ef516b1 --- /dev/null +++ b/src/bitrl/sensors/backends/range_sensor_backend_base.cpp @@ -0,0 +1,52 @@ +#include "bitrl/sensors/backends/range_sensor_backend_base.h" +#include "bitrl/sensors/backends/chrono_ultrasound_backend.h" +#include "bitrl/sensors/backends/mqtt_ultrasound_backend.h" +#include "bitrl/utils/std_map_utils.h" + +#ifdef BITRL_CHRONO +#include +#endif + +#include +namespace bitrl +{ +namespace sensors::backends +{ +std::shared_ptr RangeSensorBackendBase::create(const std::string& backend_type) +{ + + if (backend_type == "CHRONO") + { + return std::make_shared(); + } + else if (backend_type == "MQTT") + { + return std::make_shared(); + } + + throw std::runtime_error("Unknown backend type: " + backend_type); +} + +#ifdef BITRL_CHRONO +void RangeSensorBackendBase::add_visual_shape(const std::unordered_map& shape_properties) +{ + auto x_length = utils::resolve("x_length", shape_properties); + auto y_length = utils::resolve("y_length", shape_properties); + auto z_length = utils::resolve("z_length", shape_properties); + auto color = utils::resolve("chrono::ChColor", shape_properties); + auto sensor_frame = utils::resolve>("chrono::ChFrame<>", shape_properties); + + auto sensor_vis = chrono_types::make_shared(x_length, y_length, z_length); // 4cm cube + sensor_vis->SetColor(color); //chrono::ChColor(1.0f, 0.0f, 0.0f)); // red + + // chrono::ChFrame<> sensor_frame( + // chrono::ChVector3d(cd.chassis_x / 2.0 + 0.02, 0.0, cd.chassis_z / 2.0) + // ); + + // Attach to chassis + this -> body_ -> AddVisualShape(sensor_vis, sensor_frame); +} +#endif + +} +}