34 #include <boost/bind.hpp>
39 #include <geometry_msgs/Twist.h>
46 return (x > 0) ? std::min(p*x, limit) :
std::max(p*x, -limit);
52 x = std::min(x, x_old + limit);
59 move_base_actionserver_(n_,
"move_base", false)
67 pub_vel_ =
n_.advertise<geometry_msgs::Twist>(
"/cmd_vel", 1);
83 ros::AsyncSpinner spinner(1);
98 double tmp_fail_timeout;
99 n_.param<
double>(
"fail_timeout", tmp_fail_timeout, 5.0);
108 n_.param<
double>(
"p",
p_, 1.2);
117 double front, rear, left, right, tolerance;
119 n_.param(
"speed_filter_name", stName, std::string(
"/speed_filter"));
120 n_.param(stName +
"/footprint/left", left, 0.309);
121 n_.param(stName +
"/footprint/right", right, -0.309);
122 n_.param(stName +
"/footprint/front", front, 0.43);
123 n_.param(stName +
"/footprint/rear", rear, -0.43);
124 n_.param(stName +
"/footprint/tolerance", tolerance, 0.0);
142 boost::mutex::scoped_lock curr_lock(
lock);
151 boost::mutex::scoped_lock curr_lock(
lock);
153 geometry_msgs::PoseStamped goal;
157 tf_.waitForTransform(
158 global_frame_, msg.header.frame_id, msg.header.stamp, ros::Duration(1.0));
161 catch(tf::TransformException& ex)
163 ROS_WARN(
"no localization information yet %s",ex.what());
167 x_goal_ = goal.pose.position.x;
168 y_goal_ = goal.pose.position.y;
171 const geometry_msgs::Quaternion &q = goal.pose.orientation;
172 th_goal_ = atan2(q.x*q.y + q.w*q.z, 0.5 - q.y*q.y -q.z*q.z);
192 tf::StampedTransform global_pose;
197 catch(tf::TransformException& ex)
199 ROS_WARN(
"no localization information yet %s",ex.what());
204 x_now_ = global_pose.getOrigin().x();
205 y_now_ = global_pose.getOrigin().y();
208 const tf::Quaternion &q = global_pose.getRotation();
209 th_now_ = atan2(q.x()*q.y() + q.w()*q.z(), 0.5 - q.y()*q.y() - q.z()*q.z());
222 if(th_diff > M_PI) th_diff = th_diff - 2*M_PI;
223 if(th_diff < -M_PI) th_diff = th_diff + 2*M_PI;
228 double dth = th_diff;
247 #define max(A,B) (A) > (B) ? (A) : (B)
251 boost::mutex::scoped_lock curr_lock(
lock);
293 move_base_msgs::MoveBaseFeedback feedback;
294 feedback.base_position.header.stamp = ros::Time::now();
296 feedback.base_position.pose.position.x =
x_now_;
297 feedback.base_position.pose.position.y =
y_now_;
298 feedback.base_position.pose.position.z = 0.0;
299 feedback.base_position.pose.orientation = tf::createQuaternionMsgFromYaw(
th_now_);
304 #define ANG_NORM(a) atan2(sin((a)),cos((a)))
313 geometry_msgs::Twist cmdvel;
315 cmdvel.linear.x = vx;
316 cmdvel.linear.y = vy;
317 cmdvel.angular.z = vth;
324 double x2,
double y2,
double a2)
336 int main(
int argc,
char *argv[])
338 ros::init(argc, argv,
"nav_pcontroller");
ros::Duration fail_timeout_
double p_control(double x, double p, double limit)
bool retrieve_pose()
retrieves tf pose and updates (x_now_, y_now_, th_now_)
void setFootprint(double front, double rear, double left, double right, double tolerance)
Sets front_, rear_, left_, right_, tolerance_, and early_reject_distance_.
void preemptMoveBaseGoal()
actionlib::SimpleActionServer< move_base_msgs::MoveBaseAction > move_base_actionserver_
double limit_acc(double x, double x_old, double limit)
BaseDistance dist_control_
void newGoal(const geometry_msgs::PoseStamped &msg)
ros::Subscriber sub_goal_
std::string global_frame_
tf::TransformListener tf_
bool comparePoses(double x1, double y1, double a1, double x2, double y2, double a2)
void compute_distance_keeping(double *vx, double *vy, double *vth)
Sets vx_last_, vy_last and vth_last_ to input parameters, adjusting them.
ros::Time low_speed_time_
std::string base_link_frame_
int main(int argc, char *argv[])
void sendVelCmd(double vx, double vy, double vth)