//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);
}
// constructs straight-line trajectory with triangular velocity profile,
// respective limits of velocity and accel
void TrajBuilder::build_triangular_travel_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_des = atan2(dy, dx);
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
double trip_len = sqrt(dx * dx + dy * dy);
double t_ramp = sqrt(trip_len / accel_max_);
int npts_ramp = round(t_ramp / dt_);
double v_peak = accel_max_*t_ramp; // could consider special cases for reverse motion
double d_vel = alpha_max_*dt_; // incremental velocity changes for ramp-up
double x_des = x_start; //start from here
double y_des = y_start;
double speed_des = 0.0;
des_state.twist.twist.angular.z = 0.0; //omega_des; will not change
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des); //constant
// orientation of des_state will not change; only position and twist
double t = 0.0;
//ramp up;
for (int i = 0; i < npts_ramp; i++) {
t += dt_;
speed_des = accel_max_*t;
des_state.twist.twist.linear.x = speed_des; //update speed
//update positions
x_des = x_start + 0.5 * accel_max_ * t * t * cos(psi_des);
y_des = y_start + 0.5 * accel_max_ * t * t * sin(psi_des);
des_state.pose.pose.position.x = x_des;
des_state.pose.pose.position.y = y_des;
vec_of_states.push_back(des_state);
}
//ramp down:
for (int i = 0; i < npts_ramp; i++) {
speed_des -= accel_max_*dt_; //Euler one-step integration
des_state.twist.twist.linear.x = speed_des;
x_des += speed_des * dt_ * cos(psi_des); //Euler one-step integration
y_des += speed_des * dt_ * sin(psi_des); //Euler one-step integration
des_state.pose.pose.position.x = x_des;
des_state.pose.pose.position.y = y_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;
//but final orientation will follow from point-and-go direction
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
des_state.twist.twist = halt_twist_; // insist on starting from rest
vec_of_states.push_back(des_state);
}
//main fnc of this library: constructs a spin-in-place reorientation to
// point to goal coords, then a straight-line trajectory from start to goal
//NOTE: this function will clear out the vec_of_states vector of trajectory states.
// It does NOT append new planned states. User beware.
void TrajBuilder::build_point_and_go_traj(geometry_msgs::PoseStamped start_pose,
geometry_msgs::PoseStamped end_pose,
std::vector<nav_msgs::Odometry> &vec_of_states) {
ROS_INFO("building point-and-go trajectory");
nav_msgs::Odometry bridge_state;
geometry_msgs::PoseStamped bridge_pose; //bridge end of prev traj to start of new traj
vec_of_states.clear(); //get ready to build a new trajectory of desired states
ROS_INFO("building rotational trajectory");
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 des_psi = atan2(dy, dx); //heading to point towards goal pose
ROS_INFO("desired heading to subgoal = %f", des_psi);
//bridge pose: state of robot with start_x, start_y, but pointing at next subgoal
// achieve this pose with a spin move before proceeding to subgoal with translational
// motion
bridge_pose = start_pose;
bridge_pose.pose.orientation = convertPlanarPsi2Quaternion(des_psi);
ROS_INFO("building reorientation trajectory");
build_spin_traj(start_pose, bridge_pose, vec_of_states); //build trajectory to reorient
//start next segment where previous segment left off
ROS_INFO("building translational trajectory");
build_travel_traj(bridge_pose, end_pose, vec_of_states);
}
//utility to fill a PoseStamped object from planar x,y,psi info
geometry_msgs::PoseStamped SwarmControlAlgorithm::xyPsi2PoseStamped(double x, double y, double psi) {
geometry_msgs::PoseStamped poseStamped; // a pose object to populate
poseStamped.pose.orientation = convertPlanarPsi2Quaternion(psi); // convert from heading to corresponding quaternion
poseStamped.pose.position.x = x; // keep the robot on the ground!
poseStamped.pose.position.y = y; // keep the robot on the ground!
poseStamped.pose.position.z = 0.0; // keep the robot on the ground!
return poseStamped;
}
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);
}
void joint_state_CB(const sensor_msgs::JointState& joint_states) {
double dtheta_right, dtheta_left, ds, dpsi;
int n_joints = joint_states.name.size();
int ijnt;
int njnts_found = 0;
bool found_name = false;
double R_LEFT_WHEEL = R_WHEEL*LEFT_OVER_RIGHT;
double R_RIGHT_WHEEL = R_WHEEL/LEFT_OVER_RIGHT;
g_old_left_wheel_ang = g_new_left_wheel_ang;
g_old_right_wheel_ang = g_new_right_wheel_ang;
g_t_old = g_t_new;
g_cur_time = ros::Time::now();
g_t_new = g_cur_time.toSec();
g_dt = g_t_new - g_t_old;
for (ijnt = 0; ijnt < n_joints; ijnt++) {
std::string joint_name(joint_states.name[ijnt]);
if (joint_name.compare("ch1") == 0) {
g_new_left_wheel_ang = joint_states.position[ijnt];
njnts_found++;
}
if (joint_name.compare("ch2") == 0) {
g_new_right_wheel_ang = joint_states.position[ijnt];
njnts_found++;
}
}
if (njnts_found < 2) {
//ROS_WARN("did not find both wheel joint angles!");
//for (ijnt = 0; ijnt < n_joints; ijnt++) {
// std::cout<<joint_states.name[ijnt]<<std::endl;
//}
}
else {
//ROS_INFO("found both wheel joint names");
}
if (!joints_states_good) {
if (g_new_left_wheel_ang > wheel_ang_sham_init / 2.0) {
joints_states_good = true; //passed the test
g_old_left_wheel_ang = g_new_left_wheel_ang;
g_old_right_wheel_ang = g_new_right_wheel_ang; //assume right is good now as well
}
}
if (joints_states_good) { //only compute odom if wheel angles are valid
dtheta_left = g_new_left_wheel_ang - g_old_left_wheel_ang;
dtheta_right = g_new_right_wheel_ang - g_old_right_wheel_ang;
ds = 0.5 * (dtheta_left * R_LEFT_WHEEL + dtheta_right * R_RIGHT_WHEEL);
dpsi = dtheta_right * R_RIGHT_WHEEL / TRACK - dtheta_left * R_LEFT_WHEEL / TRACK;
g_drifty_odom.pose.pose.position.x += ds * cos(g_odom_psi);
g_drifty_odom.pose.pose.position.y += ds * sin(g_odom_psi);
g_odom_psi += dpsi;
//ROS_INFO("dthetal, dthetar, dpsi, odom_psi, dx, dy= %f, %f %f, %f %f %f", dtheta_left, dtheta_right, dpsi, g_odom_psi,
// ds * cos(g_odom_psi), ds * sin(g_odom_psi));
g_drifty_odom.pose.pose.orientation = convertPlanarPsi2Quaternion(g_odom_psi);
g_drifty_odom.twist.twist.linear.x = ds / g_dt;
g_drifty_odom.twist.twist.angular.z = dpsi / g_dt;
g_drifty_odom.header.stamp = g_cur_time;
g_drifty_odom_pub.publish(g_drifty_odom);
// ROS_INFO("%f",g_odom_psi/3.14159);
}
}
//this function would be useful for planning a need for sudden braking
//compute trajectory corresponding to applying max prudent decel to halt
void TrajBuilder::build_braking_traj(geometry_msgs::PoseStamped start_pose,
std::vector<nav_msgs::Odometry> &vec_of_states,nav_msgs::Odometry current_vel_states) {
ROS_INFO("We're building a braking trajectory now...");
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 = current_vel_states.twist.twist;
vec_of_states.clear();
vec_of_states.push_back(des_state);
double x_des = start_pose.pose.position.x; //start from here
double y_des = start_pose.pose.position.y;
double psi_des = convertPlanarQuat2Psi(start_pose.pose.orientation);
double speed_x = des_state.twist.twist.linear.x;
double spin_z = des_state.twist.twist.angular.z;
double stop_time = speed_x/accel_max_;
double dx = speed_x * stop_time + 0.5 * -accel_max_ * stop_time * stop_time * cos(psi_des);
double dy = speed_x * stop_time + 0.5 * -accel_max_ * stop_time * stop_time * sin(psi_des);
geometry_msgs::PoseStamped end_pose;
end_pose.pose.position.x = x_des + dx;
end_pose.pose.position.y = y_des + dy;
double speed = speed_x;
double spin =spin_z;
while(speed >0){
speed -= accel_max_*dt_; //Euler one-step integration
des_state.twist.twist.linear.x = speed;
x_des += speed * dt_ * cos(psi_des); //Euler one-step integration
y_des += speed * dt_ * sin(psi_des); //Euler one-step integration
des_state.pose.pose.position.x = x_des;
des_state.pose.pose.position.y = y_des;
vec_of_states.push_back(des_state);
}
while(spin >0){
spin -= alpha_max_*dt_; //Euler one-step integration
des_state.twist.twist.linear.z = spin;
psi_des += spin*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;
//but final orientation will follow from point-and-go direction
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
des_state.twist.twist = halt_twist_; // insist on starting from rest
vec_of_states.push_back(des_state);
/*
double x_start = start_pose.pose.position.x;
double y_start = start_pose.pose.position.y;
//time to decelerate from max speed to 0
double stop_time = speed_max_/accel_max_;
//use starting orientation because we're not turning
double psi_des = convertPlanarQuat2Psi(start_pose.pose.orientation);
//assume we start at max speed
double dx = speed_max_ * stop_time + 0.5 * -accel_max_ * stop_time * stop_time * cos(psi_des);
double dy = speed_max_ * stop_time + 0.5 * -accel_max_ * stop_time * stop_time * sin(psi_des);
geometry_msgs::PoseStamped end_pose = start_pose;
end_pose.pose.position.x = x_start + dx;
end_pose.pose.position.y = y_start + dy;
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.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des); //constant
des_state.twist.twist = halt_twist_; // insist on starting from rest
double trip_len = sqrt(dx * dx + dy * dy);
double t_ramp = sqrt(trip_len / accel_max_);
int npts_ramp = round(t_ramp / dt_);
double x_des = x_start; //start from here
double y_des = y_start;
//assume we're at max speed
double current_speed = speed_max_;
// orientation of des_state will not change; only position and twist
double t = 0.0;
//ramp down:
for (int i = 0; i < npts_ramp; i++) {
current_speed -= accel_max_*dt_; //Euler one-step integration
des_state.twist.twist.linear.x = current_speed;
x_des += current_speed * dt_ * cos(psi_des); //Euler one-step integration
y_des += current_speed * dt_ * sin(psi_des); //Euler one-step integration
des_state.pose.pose.position.x = x_des;
des_state.pose.pose.position.y = y_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;
//but final orientation will follow from point-and-go direction
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
des_state.twist.twist = halt_twist_; // insist on starting from rest
vec_of_states.push_back(des_state);
*/
}
//given that straight-line, trapezoidal velocity profile trajectory is needed,
// construct the sequence of states (positions and velocities) to achieve the
// desired motion, subject to speed and acceleration constraints
void TrajBuilder::build_trapezoidal_travel_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_des = atan2(dy, dx);
double trip_len = sqrt(dx * dx + dy * dy);
double t_ramp = speed_max_ / accel_max_;
double ramp_up_dist = 0.5 * accel_max_ * t_ramp*t_ramp;
double cruise_distance = trip_len - 2.0 * ramp_up_dist; //distance to travel at v_max
ROS_INFO("t_ramp =%f",t_ramp);
ROS_INFO("ramp-up dist = %f",ramp_up_dist);
ROS_INFO("cruise distance = %f",cruise_distance);
//start ramping up:
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
int npts_ramp = round(t_ramp / dt_);
double x_des = x_start; //start from here
double y_des = y_start;
double speed_des = 0.0;
des_state.twist.twist.angular.z = 0.0; //omega_des; will not change
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des); //constant
// orientation of des_state will not change; only position and twist
double t = 0.0;
//ramp up;
for (int i = 0; i < npts_ramp; i++) {
t += dt_;
speed_des = accel_max_*t;
des_state.twist.twist.linear.x = speed_des; //update speed
//update positions
x_des = x_start + 0.5 * accel_max_ * t * t * cos(psi_des);
y_des = y_start + 0.5 * accel_max_ * t * t * sin(psi_des);
des_state.pose.pose.position.x = x_des;
des_state.pose.pose.position.y = y_des;
vec_of_states.push_back(des_state);
}
//now cruise for distance cruise_distance at const speed
speed_des = speed_max_;
des_state.twist.twist.linear.x = speed_des;
double t_cruise = cruise_distance / speed_max_;
int npts_cruise = round(t_cruise / dt_);
ROS_INFO("t_cruise = %f; npts_cruise = %d",t_cruise,npts_cruise);
for (int i = 0; i < npts_cruise; i++) {
//Euler one-step integration
x_des += speed_des * dt_ * cos(psi_des);
y_des += speed_des * dt_ * sin(psi_des);
des_state.pose.pose.position.x = x_des;
des_state.pose.pose.position.y = y_des;
vec_of_states.push_back(des_state);
}
//ramp down:
for (int i = 0; i < npts_ramp; i++) {
speed_des -= accel_max_*dt_; //Euler one-step integration
des_state.twist.twist.linear.x = speed_des;
x_des += speed_des * dt_ * cos(psi_des); //Euler one-step integration
y_des += speed_des * dt_ * sin(psi_des); //Euler one-step integration
des_state.pose.pose.position.x = x_des;
des_state.pose.pose.position.y = y_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;
//but final orientation will follow from point-and-go direction
des_state.pose.pose.orientation = convertPlanarPsi2Quaternion(psi_des);
des_state.twist.twist = halt_twist_; // insist on starting from rest
vec_of_states.push_back(des_state);
}
