38 #include <geometry_msgs/PoseStamped.h> 39 #include <geometry_msgs/Twist.h> 40 #include <geometry_msgs/Vector3.h> 41 #include <nav_msgs/Path.h> 42 #include <std_msgs/Float32.h> 43 #include <std_msgs/Bool.h> 44 #include <move_base_msgs/MoveBaseAction.h> 47 #include <dynamic_reconfigure/server.h> 50 #include <move_basic/MovebasicConfig.h> 108 dynamic_reconfigure::Server<move_basic::MovebasicConfig>
dr_srv;
112 void goalCallback(
const geometry_msgs::PoseStamped::ConstPtr &msg);
113 void executeAction(
const move_base_msgs::MoveBaseGoalConstPtr& goal);
114 void drawLine(
double x0,
double y0,
double x1,
double y1);
115 void sendCmd(
double angular,
double linear);
118 bool getTransform(
const std::string& from,
const std::string& to,
120 bool transformPose(
const std::string& from,
const std::string& to,
129 const std::string& drivingFrame);
130 bool rotate(
double requestedYaw,
131 const std::string& drivingFrame);
141 return rad * 180.0 / M_PI;
148 return (n <0 ? -1 : 1);
217 nh.
param<std::string>(
"preferred_planning_frame",
219 nh.
param<std::string>(
"alternate_planning_frame",
221 nh.
param<std::string>(
"preferred_driving_frame",
223 nh.
param<std::string>(
"alternate_driving_frame",
229 dynamic_reconfigure::Server<move_basic::MovebasicConfig>::CallbackType
f;
254 "/move_base/goal", 1);
269 geometry_msgs::TransformStamped tfs =
317 ROS_WARN(
"Parameter change detected");
325 if (
stop)
ROS_WARN(
"MoveBasic: Robot is forced to stop!");
332 ROS_INFO(
"MoveBasic: Received simple goal");
334 move_base_msgs::MoveBaseActionGoal actionGoal;
336 actionGoal.goal.target_pose = *msg;
347 actionServer->setAborted(move_base_msgs::MoveBaseResult(), msg);
369 std::string frameId = msg->target_pose.header.frame_id;
372 if (frameId[0] ==
'/')
373 frameId = frameId.substr(1);
378 ROS_INFO(
"MoveBasic: Received goal %f %f %f %s", x, y,
rad2deg(yaw), frameId.c_str());
380 bool do_final_rotation =
true;
381 if (std::isnan(yaw)) {
382 ROS_WARN(
"MoveBasic: Received a goal with invalid orientation, will go to it but not do final turn");
383 do_final_rotation =
false;
391 std::string planningFrame;
393 planningFrame = frameId;
395 ROS_INFO(
"Planning in goal frame: %s\n", planningFrame.c_str());
401 abortGoal(
"MoveBasic: No localization available for planning");
412 ROS_INFO(
"MoveBasic: Goal in %s %f %f %f", planningFrame.c_str(),
417 geometry_msgs::PoseStamped p0, p1;
418 path.header.frame_id = frameId;
419 p0.pose.position.x = x;
420 p0.pose.position.y = y;
421 p0.header.frame_id = frameId;
422 path.poses.push_back(p0);
426 abortGoal(
"MoveBasic: Cannot determine robot pose in goal frame");
429 getPose(poseFrameId, x, y, yaw);
430 p1.pose.position.x = x;
431 p1.pose.position.y = y;
432 p1.header.frame_id = frameId;
433 path.poses.push_back(p1);
437 std::string drivingFrame;
442 ROS_WARN(
"MoveBasic: %s not available, attempting to drive using %s frame",
446 abortGoal(
"MoveBasic: Cannot determine robot pose in driving frame");
457 if (!
transformPose(frameId, drivingFrame, goal, goalInDriving)) {
458 abortGoal(
"MoveBasic: Cannot determine goal pose in driving frame");
465 getPose(goalInBase, x, y, yaw);
470 tf2::Vector3 linear = goalInBase.
getOrigin();
472 double dist = linear.length();
475 ROS_WARN(
"MoveBasic: Cannot determine robot pose for rotation");
482 double requestedYaw =
atan2(linear.y(), linear.x());
484 if (!
rotate(requestedYaw, drivingFrame)) {
491 if (!
moveLinear(goalInDriving, drivingFrame)) {
497 if (do_final_rotation) {
498 double finalYaw = goalYaw - (yaw + requestedYaw);
500 if (!
rotate(finalYaw, drivingFrame)) {
521 geometry_msgs::Twist msg;
522 msg.angular.z = angular;
523 msg.linear.x = linear;
543 geometry_msgs::Vector3 msg;
559 abortGoal(
"MoveBasic: Cannot determine robot pose for rotation");
563 double x,
y, currentYaw;
564 getPose(poseDriving, x, y, currentYaw);
565 double requestedYaw = currentYaw + yaw;
569 int oscillations = 0;
570 double prevAngleRemaining = 0;
579 double x, y, currentYaw;
582 abortGoal(
"MoveBasic: Cannot determine robot pose for rotation");
585 getPose(poseDriving, x, y, currentYaw);
587 double angleRemaining = requestedYaw - currentYaw;
591 double remaining = std::min(std::abs(angleRemaining), std::abs(obstacle));
596 if (
sign(prevAngleRemaining) !=
sign(angleRemaining)) {
599 prevAngleRemaining = angleRemaining;
602 ROS_INFO(
"MoveBasic: Stopping rotation due to preempt");
609 ROS_INFO(
"MoveBasic: Done rotation, error %f degrees",
rad2deg(angleRemaining));
614 bool counterwise = (angleRemaining < 0.0);
616 velocity = -velocity;
620 ROS_DEBUG(
"Angle remaining: %f, Angular velocity: %f",
rad2deg(angleRemaining), velocity);
628 const std::string& drivingFrame)
632 abortGoal(
"MoveBasic: Cannot determine robot pose for linear");
636 tf2::Vector3 remaining = goalInBase.
getOrigin();
639 double requestedDistance = remaining.length();
641 bool pausingForObstacle =
false;
647 double lateralIntegral = 0.0;
648 double lateralError = 0.0;
649 double prevLateralError = 0.0;
650 double lateralDiff = 0.0;
660 ROS_WARN(
"MoveBasic: Cannot determine robot pose for linear");
663 goalInBase = poseDriving * goalInDriving;
665 double distRemaining =
sqrt(remaining.x() * remaining.x() + remaining.y() * remaining.y());
668 double rotation = 0.0;
669 lateralError = remaining.y();
670 lateralDiff = lateralError - prevLateralError;
671 prevLateralError = lateralError;
672 lateralIntegral += lateralError;
678 ROS_DEBUG(
"MoveBasic: %f L %f, R %f %f %f %f %f \n",
680 remaining.x(), remaining.y(), lateralError,
684 geometry_msgs::Vector3 pid_debug;
685 pid_debug.x = remaining.x();
686 pid_debug.y = lateralError;
687 pid_debug.z = rotation;
692 if (requestedDistance < 0.0) {
701 std::min(std::min(std::abs(obstacleDist), std::abs(distRemaining)),
703 std::min(std::abs(obstacleDist), std::abs(distRemaining))))));
706 if (obstacleDetected) {
708 if (!pausingForObstacle) {
709 ROS_INFO(
"MoveBasic: PAUSING for OBSTACLE");
711 pausingForObstacle =
true;
714 ROS_INFO(
"MoveBasic: Still waiting for obstacle at %f meters!", obstacleDist);
717 abortGoal(
"MoveBasic: Aborting due to obstacle");
723 if (!obstacleDetected && pausingForObstacle) {
724 ROS_INFO(
"MoveBasic: Resuming after obstacle has gone");
725 pausingForObstacle =
false;
730 ROS_INFO(
"MoveBasic: Stopping move due to preempt");
738 double angleRemaining = std::atan2(remaining.y(), remaining.x());
739 if (std::cos(angleRemaining) < 0) {
741 abortGoal(
"MoveBasic: Moving away from goal");
754 ROS_INFO(
"MoveBasic: Done linear, error: x: %f meters, y: %f meters", remaining.x(), remaining.y());
760 ROS_DEBUG(
"Distance remaining: %f, Linear velocity: %f", distRemaining, velocity);
767 int main(
int argc,
char ** argv) {
int main(int argc, char **argv)
bool moveLinear(tf2::Transform &goalInDriving, const std::string &drivingFrame)
std::unique_ptr< CollisionChecker > collision_checker
void executeAction(const move_base_msgs::MoveBaseGoalConstPtr &goal)
std::unique_ptr< ObstaclePoints > obstacle_points
void getRPY(tf2Scalar &roll, tf2Scalar &pitch, tf2Scalar &yaw, unsigned int solution_number=1) const
double linearAcceleration
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
double maxTurningVelocity
std::string preferredPlanningFrame
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
actionlib::QueuedActionServer< move_base_msgs::MoveBaseAction > MoveBaseActionServer
std::unique_ptr< MoveBaseActionServer > actionServer
static int sign(double n)
TFSIMD_FORCE_INLINE const tfScalar & y() const
double obstacleWaitThreshold
void dynamicReconfigCallback(move_basic::MovebasicConfig &config, uint32_t)
double minTurningVelocity
double turningAcceleration
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::Publisher obstacleDistPub
bool getTransform(const std::string &from, const std::string &to, tf2::Transform &tf)
static void normalizeAngle(double &angle)
float forwardObstacleDist
void drawLine(double x0, double y0, double x1, double y1)
void fromMsg(const A &, B &b)
std::string preferredDrivingFrame
void goalCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
tf2::Transform goalInPlanning
TFSIMD_FORCE_INLINE const tfScalar & x() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string alternatePlanningFrame
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
tf2::Vector3 forwardRight
double maxLateralVelocity
void abortGoal(const std::string msg)
dynamic_reconfigure::Server< move_basic::MovebasicConfig > dr_srv
static double rad2deg(double rad)
static void getPose(const tf2::Transform &tf, double &x, double &y, double &yaw)
double localizationLatency
std::string alternateDrivingFrame
void sendCmd(double angular, double linear)
bool rotate(double requestedYaw, const std::string &drivingFrame)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
tf2_ros::TransformListener listener
void stopCallback(const std_msgs::Bool::ConstPtr &msg)
double runawayTimeoutSecs
double forwardObstacleThreshold
ROSCPP_DECL void spinOnce()
bool transformPose(const std::string &from, const std::string &to, const tf2::Transform &in, tf2::Transform &out)