/**
* @brief AdmittanceParamManager::init
* Initializes this class by starting the reconfig server.
* @param nh NodeHandle in which the reconfig server is running
*/
void AdmittanceParamManager::init(ros::NodeHandle& nh) {
// Set up dynamic reconfigure for admittance params
ros::NodeHandle nh_params(nh, "admittance_params/");
param_reconfig_server_.reset(new AdmittanceDynamicReconfigServer(param_reconfig_mutex_, nh_params));
param_reconfig_callback_ = boost::bind(&AdmittanceParamManager::dynamicReconfigCB, this, _1, _2);
param_reconfig_server_->setCallback(param_reconfig_callback_);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "controller");
ros::NodeHandle nh;
ros::NodeHandle nh_params("~");
ROS_INFO("running controller");
ros::Subscriber imu_sub = nh.subscribe("/firefly/imu", 1, &imuCallback);
ros::Subscriber pose_sub = nh.subscribe("/firefly/fake_gps/pose", 1, &poseCallback);
ros::Subscriber traj_sub = nh.subscribe("command/trajectory", 1, &MultiDofJointTrajectoryCallback);
current_index = 0;
double x_kp, x_ki, x_kd, y_kp, y_ki, y_kd, z_kp, z_ki, z_kd, yaw_kp, yaw_ki,
yaw_kd, roll_limit, pitch_limit, yaw_limit, thrust_limit;
nh_params.param("x_kp", x_kp, 1.0);
nh_params.param("x_ki", x_ki, 0.0);
nh_params.param("x_kd", x_kd, 0.0);
nh_params.param("y_kp", y_kp, 1.0);
nh_params.param("y_ki", y_ki, 0.0);
nh_params.param("y_kd", y_kd, 0.0);
nh_params.param("z_kp", z_kp, 1.0);
nh_params.param("z_ki", z_ki, 0.0);
nh_params.param("z_kd", z_kd, 0.0);
nh_params.param("yaw_kp", yaw_kp, 1.0);
nh_params.param("yaw_ki", yaw_ki, 0.0);
nh_params.param("yaw_kd", yaw_kd, 0.0);
nh_params.param("roll_limit", roll_limit, 0.0);
nh_params.param("pitch_limit", pitch_limit, 0.0);
nh_params.param("thrust_limit", thrust_limit, 0.0);
nh_params.param("yaw_limit", yaw_limit, 0.0);
// Chose one of the versions below. The first of these topic published determines the control mode.
ros::Publisher command_pub = nh.advertise<mav_msgs::RollPitchYawrateThrust>("command/roll_pitch_yawrate_thrust", 1);
// Start the dynamic_reconfigure server
dynamic_reconfigure::Server<rotors_exercise::ControllerConfig> server;
dynamic_reconfigure::Server<rotors_exercise::ControllerConfig>::CallbackType f;
f = boost::bind(&reconfigure_callback, _1, _2);
server.setCallback(f);
ROS_INFO("Initializing controller ... ");
/*
Initialize here your controllers
*/
ros::Timer timer;
timer = nh.createTimer(ros::Duration(0.2), timerCallback); //Timer for debugging
// Run the control loop and Fly to x=0m y=0m z=1m
ROS_INFO("Going to starting position [0,0,1] ...");
//positionLoop.setPoint(0.0, 0.0, 1.0, 0.0);
setpoint_pos = tf::Vector3(0.,0.,1.);
setpoint_yaw = 0.0;
latest_pose_update_time = ros::Time::now();
while(ros::ok())
{
ros::spinOnce();
delta_time_pose = (latest_pose.header.stamp - latest_pose_update_time).toSec() ;
// Check if pose/imu/state data was received
if (
(latest_pose.header.stamp.nsec > 0.0)
&&
((latest_pose.header.stamp - latest_pose_update_time).toSec() > 0.0)
)
{
latest_pose_update_time = latest_pose.header.stamp;
//compute distance to next waypoint
double distance = sqrt((setpoint_pos[0]-latest_pose.pose.position.x) * (setpoint_pos[0]-latest_pose.pose.position.x) +
(setpoint_pos[1]-latest_pose.pose.position.y) * (setpoint_pos[1]-latest_pose.pose.position.y) +
(setpoint_pos[2]-latest_pose.pose.position.z) * (setpoint_pos[2]-latest_pose.pose.position.z) );
if (distance < 0.5)
{
//there is still waypoints
if (current_index < latest_trajectory.poses.size())
{
ROS_INFO("Waypoint achieved! Moving to next waypoint");
geometry_msgs::PoseStamped wp;
wp = latest_trajectory.poses[current_index];
setpoint_pos[0]=wp.pose.position.x;
setpoint_pos[1]=wp.pose.position.y;
setpoint_pos[2]=wp.pose.position.z;
setpoint_yaw=tf::getYaw(wp.pose.orientation);
current_index++;
}else if (current_index == latest_trajectory.poses.size()) // print once waypoint achieved
{
ROS_INFO("Waypoint achieved! No more waypoints. Hovering");
current_index++;
}
}
// run position loop
double now = (double)ros::Time::now().toSec();
double delta_time = now - latest;
latest = now;
ROS_INFO_STREAM("1");
//Calculate error for each component
latest_yaw = tf::getYaw(latest_pose.pose.orientation);
ROS_INFO_STREAM("latest yaw: " << latest_yaw);
ROS_INFO_STREAM("setpoint_pos: " << setpoint_pos);
ROS_INFO_STREAM("latest_pose: " << latest_pose);
computeError(setpoint_pos,latest_pose,setpoint_yaw, latest_yaw, error);
//PROPORCIONAL Kp*error
// DONE ON GET COMMANDS
//INTEGRAL
integral(error, delta_time, integral_limits, integral_error_accumulator);
//DERIVATIVE
derivative(error,delta_time, delta_error, previous_error);
//Get commands
x_vel_cmd = x_kp*error[0] + x_ki*integral_error_accumulator[0] + x_kd*delta_error[0];
y_vel_cmd = y_kp*error[1] + y_ki*integral_error_accumulator[1] + y_kd*delta_error[1];
z_vel_cmd = z_kp*error[2] + z_ki*integral_error_accumulator[2] + z_kd*delta_error[2];
yaw_rate = yaw_kp*error[3] + yaw_ki*integral_error_accumulator[3] + yaw_kd*delta_error[3];
// your desired velocities (or accelerations) should be stored in
// x_vel_cmd,
// y_vel_cmd,
// z_vel_cmd,
// yaw_rate wrap around!!!!
//x_accel_cmd = x_vel_cmd
// Map velocities (or accelerations) to angles roll, pitch
roll_cmd = (x_vel_cmd*sin(latest_yaw) + y_vel_cmd*cos(latest_yaw))/GRAVETAT;
pitch_cmd = (x_vel_cmd*cos(latest_yaw) + y_vel_cmd*sin(latest_yaw))/GRAVETAT;
thrust = 1.5*GRAVETAT + 1.5*z_vel_cmd;
//Rotate TF from WORLD TO BODY FRAME ------ARDRONE
//tf::Vector3 vector3 (x_vel_cmd , y_vel_cmd, 0.0);
//vector3 = rotateZ (vector3, -tf::getYaw(latest_pose.pose.orientation));
//pitch_cmd = vector3[0];
//roll_cmd = vector3[1];
//thrust = z_vel_cmd;
// Saturate your request
roll_cmd = (roll_cmd > roll_limit) ? roll_limit : ((roll_cmd < -roll_limit) ? -roll_limit : roll_cmd);
pitch_cmd = (pitch_cmd > pitch_limit) ? pitch_limit : ((pitch_cmd < -pitch_limit)? -pitch_limit : pitch_cmd);
thrust = (thrust > thrust_limit) ? thrust_limit: ((thrust < -thrust_limit) ? -thrust_limit: thrust);
yaw_rate = (yaw_rate > yaw_limit) ? yaw_limit : ((yaw_rate < -yaw_limit) ? -yaw_limit : yaw_rate);
// Send to the attitude controller:
// roll angle [rad], pitch angle [rad], thrust [N][rad/s]
mav_msgs::RollPitchYawrateThrust msg;
msg.header = latest_pose.header; // use the latest information you have.
msg.roll = -roll_cmd;
msg.pitch = pitch_cmd;
msg.thrust.z = thrust;
msg.yaw_rate = yaw_rate;
command_pub.publish(msg);
}
ros::Duration(control_loop_period/2.).sleep(); // may be set slower.
}
return 0;
}
