Program Listing for File footbot_diffusion.cpp#
↰ Return to documentation for file (client/controllers/footbot_diffusion/footbot_diffusion.cpp)
/* Include the controller definition */
#include "footbot_diffusion.h"
/* Function definitions for XML parsing */
#include <argos3/core/utility/configuration/argos_configuration.h>
/* 2D vector definition */
#include <argos3/core/utility/math/vector3.h>
/****************************************/
/****************************************/
CFootBotDiffusion::CFootBotDiffusion()
: m_pcWheels(nullptr),
m_pcProximity(nullptr),
m_pcPosSens(nullptr),
m_cAlpha(10.0f),
m_angularVelocity(0.5f),
m_fWheelVelocity(2.5f),
m_cGoStraightAngleRange(-ToRadians(m_cAlpha), ToRadians(m_cAlpha)) {
}
/****************************************/
/****************************************/
void CFootBotDiffusion::insertActions(const SIM_PLAN &actions) {
// vector<action> tmp_cache;
for (const auto &action : actions) {
// cout << "not empty init" << endl;
string action1 = get<3>(action);
int nodeID = get<1>(action);
// cout << "NodeID init: " << nodeID << endl;
tuple<double, double> start_pos = get<4>(action);
double start_x = ChangeCoordinateFromMapToArgos(get<1>(start_pos));
double start_y = ChangeCoordinateFromMapToArgos(get<0>(start_pos));
tuple<double, double> end_pos = get<5>(action);
// cout << "End Position init: " << get<0>(end_pos) <<
// " " << get<1>(end_pos) << endl;
double x = ChangeCoordinateFromMapToArgos(get<1>(end_pos));
double y = ChangeCoordinateFromMapToArgos(get<0>(end_pos));
double angle;
if (get<2>(action) == 0) {
angle = 0.0;
} else if (get<2>(action) == 1) {
angle = 270.0;
} else if (get<2>(action) == 2) {
angle = 180.0;
} else {
angle = 90.0;
}
int task_id = get<6>(action);
if (robot_id == debug_id) {
cout << "Action: " << action1 << " NodeID: " << nodeID
<< " End Position: " << x << " " << y << " Angle: " << angle
<< endl;
}
if (action1 == "M") {
deque<int> prev_ids;
if (not q.empty() and q.back().type == Action::MOVE) {
prev_ids = q.back().nodeIDS;
q.pop_back();
}
prev_ids.push_back(nodeID);
q.emplace_back(x, y, angle, prev_ids, Action::MOVE);
} else if (action1 == "T") {
q.emplace_back(x, y, angle, deque<int>{nodeID}, Action::TURN);
} else if (action1 == "S") {
q.emplace_back(x, y, angle, deque<int>{nodeID}, Action::STATION,
DELIVER_T, task_id);
} else {
printf("Unknown action %s\n", action1.c_str());
exit(-2);
q.emplace_back(x, y, angle, deque<int>{nodeID}, Action::STOP);
}
}
// q.back_insert(q.begin(), tmp_cache.begin(), tmp_cache.end());
}
void CFootBotDiffusion::Init(TConfigurationNode &t_node) {
/*
* Get sensor/actuator handles
*
* The passed string (ex. "differential_steering") corresponds to the
* XML tag of the device whose handle we want to have. For a list of
* allowed values, type at the command prompt:
*
* $ argos3 -q actuators
*
* to have a list of all the possible actuators, or
*
* $ argos3 -q sensors
*
* to have a list of all the possible sensors.
*
* NOTE: ARGoS creates and initializes actuators and sensors
* internally, on the basis of the lists provided the configuration
* file at the <controllers><footbot_diffusion><actuators> and
* <controllers><footbot_diffusion><sensors> sections. If you forgot to
* list a device in the XML, and then you request it here, an error
* occurs.
*/
m_pcWheels =
GetActuator<CCI_DifferentialSteeringActuator>("differential_steering");
m_pcProximity = GetSensor<CCI_FootBotProximitySensor>("footbot_proximity");
m_pcPosSens = GetSensor<CCI_PositioningSensor>("positioning");
angular_pid_ptr = make_shared<PIDController>(1.0, 0.0, 0.1);
linear_pid_ptr = make_shared<PIDController>(2.0, 0.0, 0.1);
/*
* Parse the configuration file
*
* The user defines this part. Here, the algorithm accepts three
* parameters, and it's nice to put them in the config file so we don't
* have to recompile if we want to try other settings.
*/
GetNodeAttributeOrDefault(t_node, "alpha", m_cAlpha, m_cAlpha);
m_cGoStraightAngleRange.Set(-ToRadians(m_cAlpha), ToRadians(m_cAlpha));
GetNodeAttributeOrDefault(t_node, "omega", m_angularVelocity,
m_angularVelocity);
GetNodeAttributeOrDefault(t_node, "velocity", m_fWheelVelocity,
m_fWheelVelocity);
GetNodeAttributeOrDefault(t_node, "acceleration", m_linearAcceleration,
m_linearAcceleration);
GetNodeAttributeOrDefault(t_node, "portNumber", port_number, 8080);
GetNodeAttributeOrDefault(t_node, "simDuration", total_sim_duration, 1200);
GetNodeAttributeOrDefault(t_node, "outputDir", m_outputDir,
string("metaData/"));
GetNodeAttributeOrDefault(t_node, "screen", screen, 0);
m_linearVelocity = 1.22 * m_angularVelocity;
m_currVelocity = 0.0;
this->init_pos = m_pcPosSens->GetReading().Position;
this->robot_id = this->GetId();
// Wait until the server is ready
while (!is_port_open("127.0.0.1", port_number)) {
if (screen > 1) {
spdlog::info("Waiting for server to be ready at port: {}",
port_number);
}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
this->client = make_shared<rpc::client>("127.0.0.1", this->port_number);
this->is_initialized = true;
auto init_loc = make_tuple(
static_cast<int>(ChangeCoordinateFromArgosToMap(this->init_pos.GetY())),
static_cast<int>(
ChangeCoordinateFromArgosToMap(this->init_pos.GetX())));
this->client->call("init", robot_id, init_loc);
if (screen > 0) {
spdlog::info("Robot {} at ({},{}) connected to server at port: {}",
robot_id, get<1>(init_loc), get<0>(init_loc), port_number);
}
return;
}
/****************************************/
Real normalizeAngle(Real angle) {
while (angle > 180)
angle -= 360;
while (angle < -180)
angle += 360;
return angle;
}
double angleDifference(double curr_angle, double target_angle) {
double diff = normalizeAngle(target_angle) - normalizeAngle(curr_angle);
// Adjust the difference to be in the range [-180, 180] degrees
if (diff > 180.0) {
diff -= 360.0;
} else if (diff < -180.0) {
diff += 360.0;
}
return abs(diff);
}
/****************************************/
void CFootBotDiffusion::updateQueue() {
if (q.empty()) {
return;
}
auto first_act = q.front();
if (first_act.type == Action::MOVE) {
q.pop_front();
while (not q.empty() and q.front().type == Action::MOVE) {
auto next_act = q.front();
q.pop_front();
first_act.x = next_act.x;
first_act.y = next_act.y;
first_act.nodeIDS.insert(first_act.nodeIDS.end(),
next_act.nodeIDS.begin(),
next_act.nodeIDS.end());
}
q.push_front(first_act);
}
if (robot_id == debug_id) {
for (auto &tmp_act : q) {
cout << tmp_act.type << ", Goal position: (" << tmp_act.x << ", "
<< tmp_act.y << ")" << endl;
}
cout << "#######################" << endl;
}
}
void CFootBotDiffusion::ControlStep() {
if (not is_initialized) {
spdlog::error(
"CFootBotDiffusion::ControlStep: Controller is not initialized");
exit(-1);
return;
}
auto start = std::chrono::high_resolution_clock::now();
Action a; // current action
a.type = Action::STOP;
CQuaternion currOrient = m_pcPosSens->GetReading().Orientation;
CRadians cZAngle, cYAngle, cXAngle;
currOrient.ToEulerAngles(cZAngle, cYAngle, cXAngle);
Real currAngle = ToDegrees(cZAngle).GetValue();
if (currAngle < 0.0) {
currAngle = currAngle + 360.0;
}
Real left_v, right_v;
CVector3 currPos = m_pcPosSens->GetReading().Position;
if (count % 10 == 0) {
auto updateActions =
client->call("obtain_actions", robot_id).as<SIM_PLAN>();
if (not updateActions.empty()) {
insertActions(updateActions);
}
}
auto end_update = std::chrono::high_resolution_clock::now();
std::chrono::duration<double, milli> update_duration_ms =
end_update - start;
count++;
string receive_msg;
updateQueue();
auto end_queue_update = std::chrono::high_resolution_clock::now();
std::chrono::duration<double, milli> queue_update_duration_ms =
end_queue_update - end_update;
while (!q.empty()) {
a = q.front();
CVector3 targetPos = CVector3(a.x, a.y, 0.0f);
if (a.type == Action::MOVE && ((currPos - targetPos).Length() < EPS) and
(abs(prevVelocity_)) <= dt * m_fWheelVelocity) {
if (robot_id == debug_id) {
cout << "Confirm move when reach the goal! Remaining "
"actions num: "
<< a.nodeIDS.size() << endl;
cout << "Action: " << a.type << ", Target Position: (" << a.x
<< ", " << a.y << ")"
<< ", Current Position: (" << currPos.GetX() << ", "
<< currPos.GetY()
<< "). Previous speed is: " << prevVelocity_ << endl;
}
a.type = Action::STOP;
q.pop_front();
for (auto tmp_nodeId : a.nodeIDS) {
receive_msg =
client->call("receive_update", robot_id, tmp_nodeId)
.as<string>();
}
continue;
} else if (a.type == Action::MOVE &&
((currPos - targetPos).Length() +
MOVE_DIS * (-static_cast<double>(a.nodeIDS.size()) + 1)) <
EPS) {
if (robot_id == debug_id) {
cout << "Confirm move on the fly! Remaining actions num: "
<< a.nodeIDS.size() << endl;
cout << "Action: " << a.type << ", Target Position: (" << a.x
<< ", " << a.y << ")"
<< ", Current Position: (" << currPos.GetX() << ", "
<< currPos.GetY()
<< "). Previous speed is: " << prevVelocity_ << endl;
}
if (a.nodeIDS.size() > 1) {
receive_msg =
client->call("receive_update", robot_id, a.nodeIDS.front())
.as<string>();
q.front().nodeIDS.pop_front();
}
if ((currPos - targetPos).Length() < EPS) {
if ((abs(prevVelocity_)) <= dt * m_fWheelVelocity) {
a.type = Action::STOP;
q.pop_front();
for (auto tmp_nodeId : a.nodeIDS) {
receive_msg =
client->call("receive_update", robot_id, tmp_nodeId)
.as<string>();
}
continue;
}
}
} else if (a.type == Action::TURN &&
angleDifference(currAngle, a.angle) < 0.5f) {
a.type = Action::STOP;
receive_msg =
client->call("receive_update", robot_id, a.nodeIDS.front())
.as<string>();
q.pop_front();
continue;
} else if (a.type == Action::STATION and a.timer <= 0) {
a.type = Action::STOP;
receive_msg =
client->call("receive_update", robot_id, a.nodeIDS.front())
.as<string>();
q.pop_front();
curr_station = CVector3{-1, -1, -100};
continue;
}
break;
}
auto end_find_act = std::chrono::high_resolution_clock::now();
std::chrono::duration<double, milli> find_act_duration_ms =
end_find_act - end_queue_update;
if (a.type == Action::MOVE) {
CVector3 targetPos = CVector3(a.x, a.y, 0.0f);
pair<Real, Real> velocities = Move(targetPos, currPos, currAngle, 1.0f);
left_v = velocities.first;
right_v = velocities.second;
} else if (a.type == Action::TURN) {
// TurnLeft(a.angle, currAngle, 1.0f);
pair<Real, Real> turn_velocities = Turn(a.angle, currAngle, 1.0f);
left_v = turn_velocities.first;
right_v = turn_velocities.second;
} else if (a.type == Action::STATION) {
m_pcWheels->SetLinearVelocity(0.0f, 0.0f);
curr_station = CVector3{a.x, a.y, 0.0f};
q.front().timer--;
} else {
// stop state, waiting for next instruction
m_pcWheels->SetLinearVelocity(0.0f, 0.0f);
left_v = 0.0f;
right_v = 0.0f;
}
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double, milli> exec_duration_ms = end - end_find_act;
std::chrono::duration<double, milli> total_duration_ms = end - start;
if (total_duration_ms.count() > 1 && screen > 0) {
spdlog::info(
"Agent {}: Total time: {:.2f} ms, Update time: {:.2f} ms, "
"Queue update time: {:.2f} ms, Find action time: {:.2f} ms, "
"Execution time: {:.2f} ms, with control step count: {}",
robot_id, total_duration_ms.count(), update_duration_ms.count(),
queue_update_duration_ms.count(), find_act_duration_ms.count(),
exec_duration_ms.count(), step_count_);
}
// // Free simulation if necessary
// client->call("freeze_simulation_if_necessary", this->robot_id);
// // Loop until the simulation is defrozen
// while (client->call("is_simulation_frozen").as<bool>()) {
// // cout << "Robot " << this->robot_id
// // << ": Simulation is frozen, waiting to defrost..."
// // << endl;
// if (screen > 1) {
// // spdlog::info(
// // "Robot {}: Simulation is frozen, waiting to defrost...",
// // robot_id);
// }
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
// }
// Update sim step count in client and server.
step_count_++;
// client->call("update_sim_step", robot_id);
}
pair<Real, Real> CFootBotDiffusion::pidAngular(Real error) {
// Real error = normalizeAngle(targetAngle - currAngle);
integral_turn_error += error * dt;
Real derivative = (error - prev_turn_error) / dt;
prev_turn_error = error;
Real output = kp_turn_ * error + ki_turn_ * integral_turn_error +
kd_turn_ * derivative;
// Clamp the output to the max velocity
output = clamp(output, -m_linearVelocity, m_linearVelocity);
Real left_v = -output, right_v = output;
return make_pair(left_v, right_v);
}
Real CFootBotDiffusion::pidLinear(Real error) {
integral_move_error += error * dt;
Real derivative = (error - prev_move_error) / dt;
prev_move_error = error;
Real desiredVelocity = kp_move_ * error + ki_move_ * integral_move_error +
kd_move_ * derivative;
return desiredVelocity;
}
pair<Real, Real> CFootBotDiffusion::Turn(Real targetAngle, Real currAngle,
Real tolerance = 1.0f) {
Real error = normalizeAngle(targetAngle - currAngle);
auto turn_v = pidAngular(error);
Real left_v = turn_v.first;
Real right_v = turn_v.second;
// if ("04" == robot_id){
// cout << "PID modified velocity" << left_v<< ", " << right_v <<
// endl;
// }
// cout << "PID::Left_v: " << left_v << ", right_v: " << right_v <<
// endl;
m_pcWheels->SetLinearVelocity(left_v, right_v);
return make_pair(left_v, right_v);
}
inline Real toAngle(Real deltaX, Real deltaY) {
Real targetAngle = atan2(deltaY, deltaX);
Real tmp_angle = targetAngle / M_PI * 180.0 + 360;
if (tmp_angle > 360) {
tmp_angle -= 360;
}
assert(tmp_angle >= 0.0);
return tmp_angle;
}
double CFootBotDiffusion::getReferenceSpeed(double dist) const {
if (debug_id == robot_id)
cout << "reference dist is: " << dist << endl;
int dist_flag = 0;
if (dist < 0) {
dist_flag = -1;
} else if (dist > 0) {
dist_flag = 1;
}
return dist_flag * min(sqrt(2 * m_linearAcceleration * abs(dist) / dt),
m_fWheelVelocity);
}
pair<Real, Real> CFootBotDiffusion::Move(const CVector3 &targetPos,
const CVector3 &currPos,
Real currAngle,
Real tolerance = 1.0f) {
// Calculate the distance and angle to the target
Real deltaX = targetPos.GetX() - currPos.GetX();
Real deltaY = targetPos.GetY() - currPos.GetY();
Real distance = sqrt(deltaX * deltaX + deltaY * deltaY);
Real targetAngle = toAngle(deltaX, deltaY);
Real targetAngle2 = toAngle(-deltaX, -deltaY);
// Normalize angle difference
Real angleError1 = normalizeAngle(targetAngle - currAngle);
Real angleError2 = normalizeAngle(targetAngle2 - currAngle);
Real angleError = 0.0;
Real flag = 0.0;
// cout << "angle error 1: " << angleError1 << ", angle error 2: "
// << angleError2 << endl;
if (abs(angleError1) < abs(angleError2)) {
angleError = angleError1;
flag = 1.0;
} else {
angleError = angleError2;
flag = -1.0;
}
// PID calculations
Real refer_velocity = flag * getReferenceSpeed(distance);
// cout << "Reference velocity: " << refer_velocity << endl;
Real control_acc = pidLinear(refer_velocity - prevVelocity_);
auto angularVelocity = pidAngular(angleError);
// cout << "PID::init_l_v: " << left_v_total << ", init_r_v: " <<
// right_v_total << endl;
// Clamp the output to the max velocity
Real maxDeltaV = m_linearAcceleration * dt;
Real linearVelocity =
prevVelocity_ + clamp(control_acc, -maxDeltaV, maxDeltaV);
;
// cout << "linearVelocity: " << linearVelocity << endl;
// if (debug_id == robot_id){
// cout <<
// "PID::start###########################################################"
// << endl; cout << "Distance error: " << distance << ",
// PID::Angle Error: " << angleError << ", curr angle: " <<
// currAngle << ", target angle: " << targetAngle <<", flag
// is: " << flag << endl;
// cout << "PID::Linear Velocity: " << linearVelocity << ", refer
// vel: " << refer_velocity
// << "Prev vel: " << prevVelocity_ << endl;
// cout << "PID::Angular Velocity: " << angularVelocity.first << ",
// " << angularVelocity.second << endl; cout << "control acc:
// " << control_acc << endl; cout << "curr pos (" <<
// currPos.GetX() << ", " << currPos.GetY()
// << "). curr angle: " << currAngle
// << ", target angle: " << targetAngle
// << ", target angle2: " << targetAngle2
// << ", delta_x: " << deltaX
// << ", delta_y: " << deltaY
// << endl;
// }
Real left_v_total = linearVelocity;
Real right_v_total = linearVelocity;
// left_v_total = clamp(left_v_total, prevLeftVelocity_ - maxDeltaV,
// prevLeftVelocity_ + maxDeltaV); right_v_total =
// clamp(right_v_total, prevRightVelocity_ - maxDeltaV,
// prevRightVelocity_ + maxDeltaV);
left_v_total = clamp(left_v_total, -m_fWheelVelocity, m_fWheelVelocity);
right_v_total = clamp(right_v_total, -m_fWheelVelocity, m_fWheelVelocity);
left_v_total +=
(1 / m_fWheelVelocity) * abs(linearVelocity) * angularVelocity.first;
right_v_total +=
(1 / m_fWheelVelocity) * abs(linearVelocity) * angularVelocity.second;
// Update previous velocities for the next iteration
prevLeftVelocity_ = left_v_total;
prevRightVelocity_ = right_v_total;
prevVelocity_ = linearVelocity;
// cout << "PID::Left_v: " << left_v_total << ", right_v: " <<
// right_v_total << ", refer speed: " << refer_velocity << endl;
m_pcWheels->SetLinearVelocity(left_v_total, right_v_total);
// if (debug_id == robot_id){
// cout << "target position: " << targetPos.GetX() << ", " <<
// targetPos.GetY() << endl; cout << "PID modified velocity" <<
// left_v_total<< ", " << right_v_total << endl;
// }
return make_pair(left_v_total, right_v_total);
}
void CFootBotDiffusion::TurnLeft(Real targetAngle, Real currAngle,
Real tolerance = 1.0f) const {
Real angleDifference = targetAngle - currAngle;
Real left_v, right_v;
if (angleDifference > 0.0 && angleDifference < 180.0) {
if (abs(angleDifference) <= 10.0) {
left_v = -m_linearVelocity / (11.0 - abs(angleDifference));
right_v = +m_linearVelocity / (11.0 - abs(angleDifference));
} else {
left_v = -m_linearVelocity;
right_v = m_linearVelocity;
}
} else {
if (abs(angleDifference) <= 10.0) {
left_v = m_linearVelocity / (11.0 - abs(angleDifference));
right_v = -m_linearVelocity / (11.0 - abs(angleDifference));
} else {
left_v = m_linearVelocity;
right_v = -m_linearVelocity;
}
}
m_pcWheels->SetLinearVelocity(left_v, right_v);
}
Real CFootBotDiffusion::ChangeCoordinateFromMapToArgos(Real x) {
if (x == 0) {
return 0;
}
return -x;
}
Real CFootBotDiffusion::ChangeCoordinateFromArgosToMap(Real x) {
if (x == 0) {
return 0;
}
return -x;
}
/****************************************/
/****************************************/
/*
* This statement notifies ARGoS of the existence of the controller.
* It binds the class passed as first argument to the string passed as
* second argument.
* The string is then usable in the configuration file to refer to this
* controller.
* When ARGoS reads that string in the configuration file, it knows which
* controller class to instantiate.
* See also the configuration files for an example of how this is used.
*/
REGISTER_CONTROLLER(CFootBotDiffusion, "footbot_diffusion_controller")