// HERE IS THE BIG DEAL: USE DESIRED AND ACTUAL STATE TO COMPUTE AND PUBLISH CMD_VEL
void SteeringController::lin_steering_algorithm() {
double controller_speed;
double controller_omega;
//Eigen::Vector2d pos_err_xy_vec_;
//Eigen::Vector2d t_vec; //tangent of desired path
//Eigen::Vector2d n_vec; //normal to desired path, pointing to the "left"
double tx = cos(des_state_phi_);
double ty = sin(des_state_phi_);
double nx = -ty;
double ny = tx;
double heading_err;
double lateral_err;
double trip_dist_err; // error is scheduling...are we ahead or behind?
// have access to: des_state_vel_, des_state_omega_, des_state_x_, des_state_y_, des_state_phi_ and corresponding odom values
double dx = des_state_x_- odom_x_;
double dy = des_state_y_ - odom_y_;
//pos_err_xy_vec_ = des_xy_vec_ - odom_xy_vec_; // vector pointing from odom x-y to desired x-y
//lateral_err = n_vec.dot(pos_err_xy_vec_); //signed scalar lateral offset error; if positive, then desired state is to the left of odom
lateral_err = dx*nx + dy*ny;
trip_dist_err = dx*tx + dy*ty;
//trip_dist_err = t_vec.dot(pos_err_xy_vec_); // progress error: if positive, then we are behind schedule
heading_err = min_dang(des_state_phi_ - odom_phi_); // if positive, should rotate +omega to align with desired heading
// DEBUG OUTPUT...
// ROS_INFO("des_state_phi, odom_phi, heading err = %f, %f, %f", des_state_phi_,odom_phi_,heading_err);
// ROS_INFO("lateral err, trip dist err = %f, %f",lateral_err,trip_dist_err);
// DEFINITELY COMMENT OUT ALL cout<< OPERATIONS FOR REAL-TIME CODE
//std::cout<<des_xy_vec_<<std::endl;
//std::cout<<odom_xy_vec_<<std::endl;
// let's put these in a message to publish, for rqt_plot to display
//steering_errs_.data.clear();
//steering_errs_.data.push_back(lateral_err);
//steering_errs_.data.push_back(heading_err);
//steering_errs_.data.push_back(trip_dist_err);
//steering_errs_publisher_.publish(steering_errs_); // suitable for plotting w/ rqt_plot
//END OF DEBUG STUFF
// do something clever with this information
controller_speed = des_state_vel_ + K_TRIP_DIST*trip_dist_err; //you call that clever ?!?!?!? should speed up/slow down to null out
//controller_omega = des_state_omega_; //ditto
controller_omega = des_state_omega_ + K_PHI*heading_err + K_DISP*lateral_err;
controller_omega = MAX_OMEGA*sat(controller_omega/MAX_OMEGA); // saturate omega command at specified limits
// send out our very clever speed/spin commands:
twist_cmd_.linear.x = controller_speed;
twist_cmd_.angular.z = controller_omega;
twist_cmd2_.twist = twist_cmd_; // copy the twist command into twist2 message
twist_cmd2_.header.stamp = ros::Time::now(); // look up the time and put it in the header
cmd_publisher_.publish(twist_cmd_);
cmd_publisher2_.publish(twist_cmd2_);
}
//here are the main traj-builder fncs:
//for spin-in-place motion that would hit maximum angular velocity, construct
// trapezoidal angular velocity profile
void TrajBuilder::build_trapezoidal_spin_traj(geometry_msgs::PoseStamped start_pose,
geometry_msgs::PoseStamped end_pose,
std::vector<nav_msgs::Odometry> &vec_of_states) {
double x_start = start_pose.pose.position.x;
double y_start = start_pose.pose.position.y;
double x_end = end_pose.pose.position.x;
double y_end = end_pose.pose.position.y;
double dx = x_end - x_start;
double dy = y_end - y_start;
double psi_start = convertPlanarQuat2Psi(start_pose.pose.orientation);
double psi_end = convertPlanarQuat2Psi(end_pose.pose.orientation);
double dpsi = min_dang(psi_end - psi_start);
double t_ramp = omega_max_/ alpha_max_;
double ramp_up_dist = 0.5 * alpha_max_ * t_ramp*t_ramp;
double cruise_distance = fabs(dpsi) - 2.0 * ramp_up_dist; //delta-angle to spin at omega_max
int npts_ramp = round(t_ramp / dt_);
nav_msgs::Odometry des_state;
des_state.header = start_pose.header; //really, want to copy the frame_id
des_state.pose.pose = start_pose.pose; //start from here
des_state.twist.twist = halt_twist_; // insist on starting from rest
//ramp up omega (positive or negative);
double t = 0.0;
double accel = sgn(dpsi) * alpha_max_;
double omega_des = 0.0;
double psi_des = psi_start;
for (int i = 0; i < npts_ramp; i++) {
t += dt_;
omega_des = accel*t;
des_state.twist.twist.angular.z = omega_des; //update rotation rate
//update orientation
psi_des = psi_start + 0.5 * accel * t*t;
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
vec_of_states.push_back(des_state);
}
//now cruise for distance cruise_distance at const omega
omega_des = sgn(dpsi)*omega_max_;
des_state.twist.twist.angular.z = sgn(dpsi)*omega_max_;
double t_cruise = cruise_distance / omega_max_;
int npts_cruise = round(t_cruise / dt_);
for (int i = 0; i < npts_cruise; i++) {
//Euler one-step integration
psi_des += omega_des*dt_; //Euler one-step integration
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
vec_of_states.push_back(des_state);
}
//ramp down omega to halt:
for (int i = 0; i < npts_ramp; i++) {
omega_des -= accel*dt_; //Euler one-step integration
des_state.twist.twist.angular.z = omega_des;
psi_des += omega_des*dt_; //Euler one-step integration
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
vec_of_states.push_back(des_state);
}
//make sure the last state is precisely where desired, and at rest:
des_state.pose.pose = end_pose.pose; //
des_state.twist.twist = halt_twist_; // insist on full stop
vec_of_states.push_back(des_state);
}
void TrajBuilder::build_triangular_spin_traj(geometry_msgs::PoseStamped start_pose,
geometry_msgs::PoseStamped end_pose,
std::vector<nav_msgs::Odometry> &vec_of_states) {
nav_msgs::Odometry des_state;
des_state.header = start_pose.header; //really, want to copy the frame_id
des_state.pose.pose = start_pose.pose; //start from here
des_state.twist.twist = halt_twist_; // insist on starting from rest
vec_of_states.push_back(des_state);
double psi_start = convertPlanarQuat2Psi(start_pose.pose.orientation);
double psi_end = convertPlanarQuat2Psi(end_pose.pose.orientation);
double dpsi = min_dang(psi_end - psi_start);
ROS_INFO("spin traj: psi_start = %f; psi_end = %f; dpsi= %f", psi_start, psi_end, dpsi);
double t_ramp = sqrt(fabs(dpsi) / alpha_max_);
int npts_ramp = round(t_ramp / dt_);
double psi_des = psi_start; //start from here
double omega_des = 0.0; // assumes spin starts from rest;
// position of des_state will not change; only orientation and twist
double t = 0.0;
double accel = sgn(dpsi) * alpha_max_; //watch out for sign: CW vs CCW rotation
//ramp up;
for (int i = 0; i < npts_ramp; i++) {
t += dt_;
omega_des = accel*t;
des_state.twist.twist.angular.z = omega_des; //update rotation rate
//update orientation
psi_des = psi_start + 0.5 * accel * t*t;
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
vec_of_states.push_back(des_state);
}
//ramp down:
for (int i = 0; i < npts_ramp; i++) {
omega_des -= accel*dt_; //Euler one-step integration
des_state.twist.twist.angular.z = omega_des;
psi_des += omega_des*dt_; //Euler one-step integration
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
vec_of_states.push_back(des_state);
}
//make sure the last state is precisely where requested, and at rest:
des_state.pose.pose = end_pose.pose; //start from here
des_state.twist.twist = halt_twist_; // insist on starting from rest
vec_of_states.push_back(des_state);
}
// HERE IS THE STEERING ALGORITHM: USE DESIRED AND ACTUAL STATE TO COMPUTE AND PUBLISH CMD_VEL
void SteeringController::mobot_nl_steering() {
double controller_speed;
double controller_omega;
// have access to: des_state_vel_, des_state_omega_, des_state_x_, des_state_y_,
// des_state_phi_ and corresponding robot state values
double tx = cos(des_state_psi_); // [tx,ty] is tangent of desired path
double ty = sin(des_state_psi_);
double nx = -ty; //components [nx, ny] of normal to path, points to left of desired heading
double ny = tx;
double dx = state_x_ - des_state_x_; //x-error relative to desired path
double dy = state_y_ - des_state_y_; //y-error
lateral_err_ = dx*nx+dy*ny; //lateral error is error vector dotted with path normal
// lateral offset error is positive if robot is to the left of the path
double trip_dist_err = dx*tx+dy*ty; // progress error: if positive, then we are ahead of schedule
//heading error: if positive, should rotate -omega to align with desired heading
double heading_err = min_dang(state_psi_ - des_state_psi_);
double strategy_psi = psi_strategy(lateral_err_); //heading command, based on NL algorithm
controller_omega = omega_cmd_fnc(strategy_psi, state_psi_, des_state_psi_);
controller_speed = MAX_SPEED; //default...should speed up/slow down appropriately
// send out our speed/spin commands:
twist_cmd_.linear.x = controller_speed;
twist_cmd_.angular.z = controller_omega;
cmd_publisher_.publish(twist_cmd_);
// DEBUG OUTPUT...
ROS_INFO("des_state_phi, heading err = %f, %f", des_state_psi_,heading_err);
ROS_INFO("lateral err, trip dist err = %f, %f",lateral_err_,trip_dist_err);
std_msgs::Float32 float_msg;
float_msg.data = lateral_err_;
lat_err_publisher_.publish(float_msg);
float_msg.data = state_psi_;
heading_publisher_.publish(float_msg);
float_msg.data = psi_cmd_;
heading_cmd_publisher_.publish(float_msg);
//END OF DEBUG OUTPUT
}
//upper-level function to construct a spin-in-place trajectory
// this function decides if triangular or trapezoidal angular-velocity
// profile is needed
void TrajBuilder::build_spin_traj(geometry_msgs::PoseStamped start_pose,
geometry_msgs::PoseStamped end_pose,
std::vector<nav_msgs::Odometry> &vec_of_states) {
//decide if triangular or trapezoidal profile:
double x_start = start_pose.pose.position.x;
double y_start = start_pose.pose.position.y;
double x_end = end_pose.pose.position.x;
double y_end = end_pose.pose.position.y;
double dx = x_end - x_start;
double dy = y_end - y_start;
double psi_start = convertPlanarQuat2Psi(start_pose.pose.orientation);
double psi_end = convertPlanarQuat2Psi(end_pose.pose.orientation);
double dpsi = min_dang(psi_end - psi_start);
ROS_INFO("rotational spin distance = %f", dpsi);
double ramp_up_time = omega_max_/ alpha_max_;
double ramp_up_dist = 0.5 * alpha_max_ * ramp_up_time*ramp_up_time;
//decide on triangular vs trapezoidal:
if (fabs(dpsi) < 2.0 * ramp_up_dist) { //delta-angle is too short for trapezoid
build_triangular_spin_traj(start_pose, end_pose, vec_of_states);
} else {
build_trapezoidal_spin_traj(start_pose, end_pose, vec_of_states);
}
}