diff --git a/include/mrs_multirotor_simulator/uav_system/controllers/attitude_controller.hpp b/include/mrs_multirotor_simulator/uav_system/controllers/attitude_controller.hpp index 1876efa..12f7803 100644 --- a/include/mrs_multirotor_simulator/uav_system/controllers/attitude_controller.hpp +++ b/include/mrs_multirotor_simulator/uav_system/controllers/attitude_controller.hpp @@ -178,7 +178,7 @@ double AttitudeController::intrinsicBodyRateToHeadingRate(const Eigen::Matrix3d& // create the angular velocity tensor Eigen::Matrix3d W; - W << 0, -w[2], w[1], w[2], 0, -w[0], -w[1], w[0], 0; + W << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0; // R derivative Eigen::Matrix3d R_d = R * W; diff --git a/include/mrs_multirotor_simulator/uav_system/controllers/position_controller.hpp b/include/mrs_multirotor_simulator/uav_system/controllers/position_controller.hpp index 1d5089d..7f2d691 100644 --- a/include/mrs_multirotor_simulator/uav_system/controllers/position_controller.hpp +++ b/include/mrs_multirotor_simulator/uav_system/controllers/position_controller.hpp @@ -76,9 +76,9 @@ reference::VelocityHdg PositionController::getControlSignal(const MultirotorMode reference::VelocityHdg output; - output.velocity[0] = pid_x_.update(pos_error[0], dt); - output.velocity[1] = pid_y_.update(pos_error[1], dt); - output.velocity[2] = pid_z_.update(pos_error[2], dt); + output.velocity(0) = pid_x_.update(pos_error(0), dt); + output.velocity(1) = pid_y_.update(pos_error(1), dt); + output.velocity(2) = pid_z_.update(pos_error(2), dt); output.heading = reference.heading; diff --git a/include/mrs_multirotor_simulator/uav_system/controllers/velocity_controller.hpp b/include/mrs_multirotor_simulator/uav_system/controllers/velocity_controller.hpp index bb2576d..4338dba 100644 --- a/include/mrs_multirotor_simulator/uav_system/controllers/velocity_controller.hpp +++ b/include/mrs_multirotor_simulator/uav_system/controllers/velocity_controller.hpp @@ -72,9 +72,9 @@ reference::AccelerationHdg VelocityController::getControlSignal(const Multirotor reference::AccelerationHdg output; - output.acceleration[0] = pid_x_.update(vel_error[0], dt); - output.acceleration[1] = pid_y_.update(vel_error[1], dt); - output.acceleration[2] = pid_z_.update(vel_error[2], dt); + output.acceleration(0) = pid_x_.update(vel_error(0), dt); + output.acceleration(1) = pid_y_.update(vel_error(1), dt); + output.acceleration(2) = pid_z_.update(vel_error(2), dt); output.heading = reference.heading; @@ -92,9 +92,9 @@ reference::AccelerationHdgRate VelocityController::getControlSignal(const Multir reference::AccelerationHdgRate output; - output.acceleration[0] = pid_x_.update(vel_error[0], dt); - output.acceleration[1] = pid_y_.update(vel_error[1], dt); - output.acceleration[2] = pid_z_.update(vel_error[2], dt); + output.acceleration(0) = pid_x_.update(vel_error(0), dt); + output.acceleration(1) = pid_y_.update(vel_error(1), dt); + output.acceleration(2) = pid_z_.update(vel_error(2), dt); output.heading_rate = reference.heading_rate; diff --git a/include/mrs_multirotor_simulator/uav_system/multirotor_model.hpp b/include/mrs_multirotor_simulator/uav_system/multirotor_model.hpp index 930ab25..70a3c7f 100644 --- a/include/mrs_multirotor_simulator/uav_system/multirotor_model.hpp +++ b/include/mrs_multirotor_simulator/uav_system/multirotor_model.hpp @@ -203,12 +203,12 @@ void MultirotorModel::initializeState(void) { void MultirotorModel::updateInternalState(void) { for (int i = 0; i < 3; i++) { - internal_state_[0 + i] = state_.x(i); - internal_state_[3 + i] = state_.v(i); - internal_state_[6 + i] = state_.R(i, 0); - internal_state_[9 + i] = state_.R(i, 1); - internal_state_[12 + i] = state_.R(i, 2); - internal_state_[15 + i] = state_.omega(i); + internal_state_.at(0 + i) = state_.x(i); + internal_state_.at(3 + i) = state_.v(i); + internal_state_.at(6 + i) = state_.R(i, 0); + internal_state_.at(9 + i) = state_.R(i, 1); + internal_state_.at(12 + i) = state_.R(i, 2); + internal_state_.at(15 + i) = state_.omega(i); } } @@ -225,19 +225,19 @@ void MultirotorModel::step(const double& dt) { odeint::integrate_n_steps(rk, boost::ref(*this), internal_state_, 0.0, dt, 1); for (int i = 0; i < N_INTERNAL_STATES; ++i) { - if (std::isnan(internal_state_[i])) { + if (std::isnan(internal_state_.at(i))) { internal_state_ = save; break; } } for (int i = 0; i < 3; i++) { - state_.x(i) = internal_state_[0 + i]; - state_.v(i) = internal_state_[3 + i]; - state_.R(i, 0) = internal_state_[6 + i]; - state_.R(i, 1) = internal_state_[9 + i]; - state_.R(i, 2) = internal_state_[12 + i]; - state_.omega(i) = internal_state_[15 + i]; + state_.x(i) = internal_state_.at(0 + i); + state_.v(i) = internal_state_.at(3 + i); + state_.R(i, 0) = internal_state_.at(6 + i); + state_.R(i, 1) = internal_state_.at(9 + i); + state_.R(i, 2) = internal_state_.at(12 + i); + state_.omega(i) = internal_state_.at(15 + i); } double filter_const = exp((-dt) / (params_.motor_time_constant)); @@ -265,8 +265,8 @@ void MultirotorModel::step(const double& dt) { const double hover_rpm = sqrt((params_.mass * params_.g) / (params_.n_motors * params_.kf)); if (input_.mean() <= 0.90 * hover_rpm) { - if (state_.x(2) < _initial_pos_[2] && state_.v(2) < 0) { - state_.x(2) = _initial_pos_[2]; + if (state_.x(2) < _initial_pos_(2) && state_.v(2) < 0) { + state_.x(2) = _initial_pos_(2); state_.v = Eigen::Vector3d::Zero(); state_.omega = Eigen::Vector3d::Zero(); } @@ -302,12 +302,12 @@ void MultirotorModel::operator()(const MultirotorModel::InternalState& x, Multir State cur_state; for (int i = 0; i < 3; i++) { - cur_state.x(i) = x[0 + i]; - cur_state.v(i) = x[3 + i]; - cur_state.R(i, 0) = x[6 + i]; - cur_state.R(i, 1) = x[9 + i]; - cur_state.R(i, 2) = x[12 + i]; - cur_state.omega(i) = x[15 + i]; + cur_state.x(i) = x.at(0 + i); + cur_state.v(i) = x.at(3 + i); + cur_state.R(i, 0) = x.at(6 + i); + cur_state.R(i, 1) = x.at(9 + i); + cur_state.R(i, 2) = x.at(12 + i); + cur_state.omega(i) = x.at(15 + i); } Eigen::LLT llt(cur_state.R.transpose() * cur_state.R); @@ -349,17 +349,17 @@ void MultirotorModel::operator()(const MultirotorModel::InternalState& x, Multir omega_dot = params_.J.inverse() * (torque_thrust.topRows(3) - cur_state.omega.cross(params_.J * cur_state.omega) + external_moment_); for (int i = 0; i < 3; i++) { - dxdt[0 + i] = x_dot(i); - dxdt[3 + i] = v_dot(i); - dxdt[6 + i] = R_dot(i, 0); - dxdt[9 + i] = R_dot(i, 1); - dxdt[12 + i] = R_dot(i, 2); - dxdt[15 + i] = omega_dot(i); + dxdt.at(0 + i) = x_dot(i); + dxdt.at(3 + i) = v_dot(i); + dxdt.at(6 + i) = R_dot(i, 0); + dxdt.at(9 + i) = R_dot(i, 1); + dxdt.at(12 + i) = R_dot(i, 2); + dxdt.at(15 + i) = omega_dot(i); } for (int i = 0; i < N_INTERNAL_STATES; ++i) { - if (std::isnan(dxdt[i])) { - dxdt[i] = 0; + if (std::isnan(dxdt.at(i))) { + dxdt.at(i) = 0; } } } diff --git a/src/multirotor_simulator.cpp b/src/multirotor_simulator.cpp index 412c4cb..f4f4927 100644 --- a/src/multirotor_simulator.cpp +++ b/src/multirotor_simulator.cpp @@ -149,7 +149,7 @@ void MultirotorSimulator::onInit() { for (size_t i = 0; i < uav_names.size(); i++) { - std::string uav_name = uav_names[i]; + std::string uav_name = uav_names.at(i); ROS_INFO("[MultirotorSimulator]: initializing '%s'", uav_name.c_str()); @@ -209,7 +209,7 @@ void MultirotorSimulator::timerMain([[maybe_unused]] const ros::WallTimerEvent& sim_time_ = sim_time_ + ros::Duration(simulation_step_size); for (size_t i = 0; i < uavs_.size(); i++) { - uavs_[i]->makeStep(simulation_step_size); + uavs_.at(i)->makeStep(simulation_step_size); } publishPoses(); @@ -303,7 +303,7 @@ void MultirotorSimulator::handleCollisions(void) { std::vector poses; for (size_t i = 0; i < uavs_.size(); i++) { - poses.push_back(uavs_[i]->getPose()); + poses.push_back(uavs_.at(i)->getPose()); } typedef KDTreeVectorOfVectorsAdaptor my_kd_tree_t; @@ -320,24 +320,24 @@ void MultirotorSimulator::handleCollisions(void) { for (size_t i = 0; i < uavs_.size(); i++) { - MultirotorModel::State state_1 = uavs_[i]->getState(); - MultirotorModel::ModelParams params_1 = uavs_[i]->getParams(); + MultirotorModel::State state_1 = uavs_.at(i)->getState(); + MultirotorModel::ModelParams params_1 = uavs_.at(i)->getParams(); nanoflann::RadiusResultSet resultSet(3.0, indices_dists); - mat_index.index->findNeighbors(resultSet, &state_1.x[0]); + mat_index.index->findNeighbors(resultSet, &state_1.x(0)); for (size_t j = 0; j < resultSet.m_indices_dists.size(); j++) { - const size_t idx = resultSet.m_indices_dists[j].first; - const double dist = resultSet.m_indices_dists[j].second; + const size_t idx = resultSet.m_indices_dists.at(j).first; + const double dist = resultSet.m_indices_dists.at(j).second; if (idx == i) { continue; } - MultirotorModel::State state_2 = uavs_[idx]->getState(); - MultirotorModel::ModelParams params_2 = uavs_[idx]->getParams(); + MultirotorModel::State state_2 = uavs_.at(idx)->getState(); + MultirotorModel::ModelParams params_2 = uavs_.at(idx)->getParams(); const double crit_dist = params_1.arm_length + params_1.prop_radius + params_2.arm_length + params_2.prop_radius; @@ -345,16 +345,16 @@ void MultirotorSimulator::handleCollisions(void) { if (dist < crit_dist) { if (drs_params.collisions_crash) { - uavs_[idx]->crash(); + uavs_.at(idx)->crash(); } else { - forces[i] += drs_params.collisions_rebounce * rel_pos.normalized() * params_1.mass * (params_2.mass / (params_1.mass + params_2.mass)); + forces.at(i) += drs_params.collisions_rebounce * rel_pos.normalized() * params_1.mass * (params_2.mass / (params_1.mass + params_2.mass)); } } } } for (size_t i = 0; i < uavs_.size(); i++) { - uavs_[i]->applyForce(forces[i]); + uavs_.at(i)->applyForce(forces.at(i)); } } @@ -373,13 +373,13 @@ void MultirotorSimulator::publishPoses(void) { for (size_t i = 0; i < uavs_.size(); i++) { - auto state = uavs_[i]->getState(); + auto state = uavs_.at(i)->getState(); geometry_msgs::Pose pose; - pose.position.x = state.x[0]; - pose.position.y = state.x[1]; - pose.position.z = state.x[2]; + pose.position.x = state.x(0); + pose.position.y = state.x(1); + pose.position.z = state.x(2); pose.orientation = mrs_lib::AttitudeConverter(state.R); pose_array.poses.push_back(pose); diff --git a/src/uav_system_ros.cpp b/src/uav_system_ros.cpp index 47ec4fd..f5795af 100644 --- a/src/uav_system_ros.cpp +++ b/src/uav_system_ros.cpp @@ -349,19 +349,19 @@ void UavSystemRos::publishOdometry(const MultirotorModel::State &state) { odom.pose.pose.orientation = mrs_lib::AttitudeConverter(state.R); - odom.pose.pose.position.x = state.x[0]; - odom.pose.pose.position.y = state.x[1]; - odom.pose.pose.position.z = state.x[2]; + odom.pose.pose.position.x = state.x(0); + odom.pose.pose.position.y = state.x(1); + odom.pose.pose.position.z = state.x(2); Eigen::Vector3d vel_body = state.R.transpose() * state.v; - odom.twist.twist.linear.x = vel_body[0]; - odom.twist.twist.linear.y = vel_body[1]; - odom.twist.twist.linear.z = vel_body[2]; + odom.twist.twist.linear.x = vel_body(0); + odom.twist.twist.linear.y = vel_body(1); + odom.twist.twist.linear.z = vel_body(2); - odom.twist.twist.angular.x = state.omega[0]; - odom.twist.twist.angular.y = state.omega[1]; - odom.twist.twist.angular.z = state.omega[2]; + odom.twist.twist.angular.x = state.omega(0); + odom.twist.twist.angular.y = state.omega(1); + odom.twist.twist.angular.z = state.omega(2); ph_odom_.publish(odom); } @@ -377,15 +377,15 @@ void UavSystemRos::publishIMU(const MultirotorModel::State &state) { imu.header.stamp = ros::Time::now(); imu.header.frame_id = _frame_fcu_; - imu.angular_velocity.x = state.omega[0]; - imu.angular_velocity.y = state.omega[1]; - imu.angular_velocity.z = state.omega[2]; + imu.angular_velocity.x = state.omega(0); + imu.angular_velocity.y = state.omega(1); + imu.angular_velocity.z = state.omega(2); auto acc = uav_system_.getImuAcceleration(); - imu.linear_acceleration.x = acc[0]; - imu.linear_acceleration.y = acc[1]; - imu.linear_acceleration.z = acc[2]; + imu.linear_acceleration.x = acc(0); + imu.linear_acceleration.y = acc(1); + imu.linear_acceleration.z = acc(2); ph_imu_.publish(imu); } @@ -406,8 +406,8 @@ void UavSystemRos::publishRangefinder(const MultirotorModel::State &state) { double range_measurement; - if (body_z[2] > 0) { - range_measurement = (state.x[2] - model_params_.ground_z) / cos(tilt); + if (body_z(2) > 0) { + range_measurement = (state.x(2) - model_params_.ground_z) / cos(tilt); } else { range_measurement = std::numeric_limits::max(); } @@ -453,9 +453,9 @@ void UavSystemRos::publishRangefinder(const MultirotorModel::State &state) { tf.header.frame_id = _frame_world_; tf.child_frame_id = _frame_fcu_; - tf.transform.translation.x = state.x[0]; - tf.transform.translation.y = state.x[1]; - tf.transform.translation.z = state.x[2]; + tf.transform.translation.x = state.x(0); + tf.transform.translation.y = state.x(1); + tf.transform.translation.z = state.x(2); tf.transform.rotation = mrs_lib::AttitudeConverter(state.R); @@ -693,7 +693,7 @@ void UavSystemRos::callbackActuatorCmd(const mrs_msgs::HwApiActuatorCmd::ConstPt cmd.motors = Eigen::VectorXd::Zero(model_params_.n_motors); for (int i = 0; i < model_params_.n_motors; i++) { - cmd.motors[i] = msg->motors[i]; + cmd.motors(i) = msg->motors.at(i); } { @@ -824,9 +824,9 @@ void UavSystemRos::callbackAccelerationHdgRateCmd(const mrs_msgs::HwApiAccelerat cmd.heading_rate = msg->heading_rate; - cmd.acceleration[0] = msg->acceleration.x; - cmd.acceleration[1] = msg->acceleration.y; - cmd.acceleration[2] = msg->acceleration.z; + cmd.acceleration(0) = msg->acceleration.x; + cmd.acceleration(1) = msg->acceleration.y; + cmd.acceleration(2) = msg->acceleration.z; { std::scoped_lock lock(mutex_uav_system_); @@ -858,9 +858,9 @@ void UavSystemRos::callbackAccelerationHdgCmd(const mrs_msgs::HwApiAccelerationH cmd.heading = msg->heading; - cmd.acceleration[0] = msg->acceleration.x; - cmd.acceleration[1] = msg->acceleration.y; - cmd.acceleration[2] = msg->acceleration.z; + cmd.acceleration(0) = msg->acceleration.x; + cmd.acceleration(1) = msg->acceleration.y; + cmd.acceleration(2) = msg->acceleration.z; { std::scoped_lock lock(mutex_uav_system_); @@ -892,9 +892,9 @@ void UavSystemRos::callbackVelocityHdgRateCmd(const mrs_msgs::HwApiVelocityHdgRa cmd.heading_rate = msg->heading_rate; - cmd.velocity[0] = msg->velocity.x; - cmd.velocity[1] = msg->velocity.y; - cmd.velocity[2] = msg->velocity.z; + cmd.velocity(0) = msg->velocity.x; + cmd.velocity(1) = msg->velocity.y; + cmd.velocity(2) = msg->velocity.z; { std::scoped_lock lock(mutex_uav_system_); @@ -926,9 +926,9 @@ void UavSystemRos::callbackVelocityHdgCmd(const mrs_msgs::HwApiVelocityHdgCmd::C cmd.heading = msg->heading; - cmd.velocity[0] = msg->velocity.x; - cmd.velocity[1] = msg->velocity.y; - cmd.velocity[2] = msg->velocity.z; + cmd.velocity(0) = msg->velocity.x; + cmd.velocity(1) = msg->velocity.y; + cmd.velocity(2) = msg->velocity.z; { std::scoped_lock lock(mutex_uav_system_); @@ -960,9 +960,9 @@ void UavSystemRos::callbackPositionCmd(const mrs_msgs::HwApiPositionCmd::ConstPt cmd.heading = msg->heading; - cmd.position[0] = msg->position.x; - cmd.position[1] = msg->position.y; - cmd.position[2] = msg->position.z; + cmd.position(0) = msg->position.x; + cmd.position(1) = msg->position.y; + cmd.position(2) = msg->position.z; { std::scoped_lock lock(mutex_uav_system_); @@ -995,12 +995,12 @@ void UavSystemRos::callbackTrackerCmd(const mrs_msgs::TrackerCommand::ConstPtr m double heading_rate = 0; if (msg->use_velocity_horizontal) { - velocity[0] = msg->velocity.x; - velocity[1] = msg->velocity.y; + velocity(0) = msg->velocity.x; + velocity(1) = msg->velocity.y; } if (msg->use_velocity_vertical) { - velocity[2] = msg->velocity.z; + velocity(2) = msg->velocity.z; } if (msg->use_heading_rate) { @@ -1008,9 +1008,9 @@ void UavSystemRos::callbackTrackerCmd(const mrs_msgs::TrackerCommand::ConstPtr m } if (msg->use_acceleration) { - acceleration[0] = msg->acceleration.x; - acceleration[1] = msg->acceleration.y; - acceleration[2] = msg->acceleration.z; + acceleration(0) = msg->acceleration.x; + acceleration(1) = msg->acceleration.y; + acceleration(2) = msg->acceleration.z; } uav_system_.setFeedforward(reference::VelocityHdg(velocity, 0));