VehicleAckermann_ControllerRaw.cpp
Go to the documentation of this file.
00001 /*+-------------------------------------------------------------------------+
00002   |                       MultiVehicle simulator (libmvsim)                 |
00003   |                                                                         |
00004   | Copyright (C) 2014  Jose Luis Blanco Claraco (University of Almeria)    |
00005   | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
00006   | Distributed under GNU General Public License version 3                  |
00007   |   See <http://www.gnu.org/licenses/>                                    |
00008   +-------------------------------------------------------------------------+ */
00009 
00010 #include <mvsim/VehicleDynamics/VehicleAckermann.h>
00011 #include "xml_utils.h"
00012 
00013 using namespace mvsim;
00014 using namespace std;
00015 
00016 DynamicsAckermann::ControllerRawForces::ControllerRawForces(
00017         DynamicsAckermann& veh)
00018         : ControllerBase(veh),
00019           setpoint_wheel_torque_l(0),
00020           setpoint_wheel_torque_r(0),
00021           setpoint_steer_ang(0)
00022 {
00023 }
00024 
00025 // See base class docs
00026 void DynamicsAckermann::ControllerRawForces::control_step(
00027         const DynamicsAckermann::TControllerInput& ci,
00028         DynamicsAckermann::TControllerOutput& co)
00029 {
00030         co.fl_torque = this->setpoint_wheel_torque_l;
00031         co.fr_torque = this->setpoint_wheel_torque_r;
00032         co.steer_ang = this->setpoint_steer_ang;
00033 }
00034 
00035 void DynamicsAckermann::ControllerRawForces::load_config(
00036         const rapidxml::xml_node<char>& node)
00037 {
00038         std::map<std::string, TParamEntry> params;
00039         params["fl_torque"] = TParamEntry("%lf", &setpoint_wheel_torque_l);
00040         params["fr_torque"] = TParamEntry("%lf", &setpoint_wheel_torque_r);
00041 
00042         // Initial speed.
00043         params["steer_ang_deg"] = TParamEntry("%lf_deg", &this->setpoint_steer_ang);
00044 
00045         parse_xmlnode_children_as_param(node, params);
00046 }
00047 
00048 void DynamicsAckermann::ControllerRawForces::teleop_interface(
00049         const TeleopInput& in, TeleopOutput& out)
00050 {
00051         ControllerBase::teleop_interface(in, out);
00052 
00053         switch (in.keycode)
00054         {
00055                 case 'W':
00056                 case 'w':
00057                         setpoint_wheel_torque_l -= 1.0;
00058                         setpoint_wheel_torque_r -= 1.0;
00059                         break;
00060 
00061                 case 'S':
00062                 case 's':
00063                         setpoint_wheel_torque_l += 1.0;
00064                         setpoint_wheel_torque_r += 1.0;
00065                         break;
00066 
00067                 case 'A':
00068                 case 'a':
00069                         setpoint_steer_ang += 1.0 * M_PI / 180.0;
00070                         keep_min(
00071                                 setpoint_steer_ang, m_veh.getMaxSteeringAngle());
00072                         break;
00073 
00074                 case 'D':
00075                 case 'd':
00076                         setpoint_steer_ang -= 1.0 * M_PI / 180.0;
00077                         keep_max(
00078                                 setpoint_steer_ang, -m_veh.getMaxSteeringAngle());
00079                         break;
00080 
00081                 case ' ':
00082                         setpoint_wheel_torque_l = .0;
00083                         setpoint_wheel_torque_r = .0;
00084                         break;
00085         };
00086         out.append_gui_lines += "[Controller=" + string(class_name()) +
00087                                                         "] Teleop keys: w/s=incr/decr torques. "
00088                                                         "a/d=left/right steering. spacebar=stop.\n";
00089         out.append_gui_lines += mrpt::format(
00090                 "setpoint: t=%.03f steer=%.03f deg\n", setpoint_wheel_torque_l,
00091                 setpoint_steer_ang * 180.0 / M_PI);
00092 }


mvsim
Author(s):
autogenerated on Thu Jun 6 2019 22:08:35