void PoseFollower::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros){
tf_ = tf;
costmap_ros_ = costmap_ros;
current_waypoint_ = 0;
goal_reached_time_ = ros::Time::now();
ros::NodeHandle node_private("~/" + name);
collision_planner_.initialize(name, tf_, costmap_ros_);
node_private.param("k_trans", K_trans_, 2.0);
node_private.param("k_rot", K_rot_, 2.0);
//within this distance to the goal, finally rotate to the goal heading (also, we've reached our goal only if we're within this dist)
node_private.param("tolerance_trans", tolerance_trans_, 0.02);
//we've reached our goal only if we're within this angular distance
node_private.param("tolerance_rot", tolerance_rot_, 0.04);
//we've reached our goal only if we're within range for this long and stopped
node_private.param("tolerance_timeout", tolerance_timeout_, 0.5);
//set this to true if you're using a holonomic robot
node_private.param("holonomic", holonomic_, true);
//number of samples (scaling factors of our current desired twist) to check the validity of
node_private.param("samples", samples_, 10);
//go no faster than this
node_private.param("max_vel_lin", max_vel_lin_, 0.9);
node_private.param("max_vel_th", max_vel_th_, 1.4);
//minimum velocities to keep from getting stuck
node_private.param("min_vel_lin", min_vel_lin_, 0.1);
node_private.param("min_vel_th", min_vel_th_, 0.0);
//if we're rotating in place, go at least this fast to avoid getting stuck
node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
//when we're near the end and would be trying to go no faster than this translationally, just rotate in place instead
node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
//we're "stopped" if we're going slower than these velocities
node_private.param("trans_stopped_velocity", trans_stopped_velocity_, 1e-4);
node_private.param("rot_stopped_velocity", rot_stopped_velocity_, 1e-4);
//if this is true, we don't care whether we go backwards or forwards
node_private.param("allow_backwards", allow_backwards_, false);
//if this is true, turn in place to face the new goal instead of arcing toward it
node_private.param("turn_in_place_first", turn_in_place_first_, false);
//if turn_in_place_first is true, turn in place if our heading is more than this far from facing the goal location
node_private.param("max_heading_diff_before_moving", max_heading_diff_before_moving_, 0.17);
ros::NodeHandle node;
odom_sub_ = node.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&PoseFollower::odomCallback, this, _1));
vel_pub_ = node.advertise<geometry_msgs::Twist>("pf/cmd_vel", 10);
ROS_DEBUG("Initialized");
}
void PoseFollower::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) {
tf_ = tf;
costmap_ros_ = costmap_ros;
current_waypoint_ = 0;
goal_reached_time_ = ros::Time::now();
ros::NodeHandle node_private("~/" + name);
collision_planner_.initialize(name, tf_, costmap_ros_);
node_private.param("k_trans", K_trans_, 3.0);
node_private.param("k_rot", K_rot_, 3.0);
//----------------- Paramter modified ----------------------------
node_private.param("tolerance_trans", tolerance_trans_, 0.35); // War 0.3 geändert 20.1. CPF
node_private.param("tolerance_rot", tolerance_rot_, 1.50); // War 0.3 geändert 20.1. CPF
node_private.param("tolerance_timeout", tolerance_timeout_, 1.5);
node_private.param("holonomic", holonomic_, false);
node_private.param("samples", samples_, 100);
node_private.param("max_vel_lin", max_vel_lin_, 0.3);
node_private.param("max_vel_th", max_vel_th_, 1.0);
node_private.param("min_vel_lin", min_vel_lin_, 0.3);
node_private.param("min_vel_th", min_vel_th_, -0.3);
node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
node_private.param("trans_stopped_velocity", trans_stopped_velocity_, 1e-4);
node_private.param("rot_stopped_velocity", rot_stopped_velocity_, 1e-4);
node_private.param("lookahead_count", lookahead_count_, 1);
node_private.param("lookahead_weight", lookahead_weight_, 0.2);
ros::NodeHandle node;
odom_sub_ = node.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&PoseFollower::odomCallback, this, _1));
vel_pub_ = node.advertise<geometry_msgs::Twist>("cmd_vel", 10);
ROS_DEBUG("Initialized");
}
void HectorPathFollower::initialize(tf::TransformListener* tf){
tf_ = tf;
current_waypoint_ = 0;
goal_reached_time_ = ros::Time::now();
ros::NodeHandle node_private("~/");
node_private.param("k_trans", K_trans_, 2.0);
node_private.param("k_rot", K_rot_, 2.0);
node_private.param("tolerance_trans", tolerance_trans_, 0.1);
node_private.param("tolerance_rot", tolerance_rot_, 0.2);
node_private.param("tolerance_timeout", tolerance_timeout_, 0.5);
node_private.param("holonomic", holonomic_, false);
node_private.param("samples", samples_, 10);
node_private.param("max_vel_lin", max_vel_lin_, 0.9);
node_private.param("max_vel_th", max_vel_th_, 1.4);
node_private.param("min_vel_lin", min_vel_lin_, 0.1);
node_private.param("min_vel_th", min_vel_th_, 0.0);
node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
node_private.param("trans_stopped_velocity", trans_stopped_velocity_, 1e-4);
node_private.param("rot_stopped_velocity", rot_stopped_velocity_, 1e-4);
node_private.param("robot_base_frame", p_robot_base_frame_, std::string("base_link"));
node_private.param("global_frame", p_global_frame_, std::string("map"));
//ros::NodeHandle node;
//vel_pub_ = node.advertise<geometry_msgs::Twist>("cmd_vel", 10);
ROS_DEBUG("Initialized");
}
