Public Member Functions | |
bool | getRobotPose (tf::StampedTransform &globalToBase, const std::string &global_frame_id) |
void | goalActionCB (const move_base_msgs::MoveBaseGoalConstPtr &goal) |
void | goalCB (const geometry_msgs::PoseStampedConstPtr &goal) |
void | pathActionCB (const naoqi_bridge_msgs::FollowPathGoalConstPtr &goal) |
void | pathCB (const nav_msgs::PathConstPtr &goal) |
PathFollower () | |
~PathFollower () | |
Private Member Functions | |
void | footContactCallback (const std_msgs::BoolConstPtr &contact) |
bool | getNextTarget (const nav_msgs::Path &path, const tf::Pose &robotPose, const std::vector< geometry_msgs::PoseStamped >::const_iterator ¤tPathPoseIt, tf::Stamped< tf::Transform > &targetPose, std::vector< geometry_msgs::PoseStamped >::const_iterator &targetPathPoseIt) |
void | inhibitJoystick () |
void | publishTargetPoseVis (const tf::Stamped< tf::Pose > &targetPose) |
Publishes the target pose as a geometry_msgs/Pose for visualization. | |
void | setVelocity (const tf::Transform &relTarget) |
Velocity controller. | |
void | stopWalk () |
Stop walk and uninhibit joystick. | |
Private Attributes | |
ros::Publisher | m_actionGoalPub |
ros::Publisher | m_actionPathPub |
std::string | m_baseFrameId |
Base frame of the robot. | |
double | m_controllerFreq |
Desired frequency for controller loop. | |
bool | m_footContact |
True = robot currently has foot contact with the ground. | |
ros::Subscriber | m_footContactSub |
ros::ServiceClient | m_inhibitWalkClient |
bool | m_isJoystickInhibited |
True = joystick walk commands are inhibited. | |
const double | m_maxStepFreq |
Maximum step frequency (2.38 Hz according to Nao documentation). | |
double | m_maxVelFractionX |
Maximum fraction of forward velocity for velocity controller, between 0.0 and 1.0. | |
double | m_maxVelFractionY |
Maximum fraction of sideways velocity for velocity controller, between 0.0 and 1.0. | |
double | m_maxVelFractionYaw |
Maximum fraction of rotational velocity for velocity controller, between 0.0 and 1.0. | |
const double | m_maxVelXY |
Maximum translational velocity (0.0952 m/s according to Nao documentation). | |
const double | m_maxVelYaw |
Maximum rotational velocity (47.6 deg/s according to Nao documentation). | |
const double | m_minStepFreq |
Minimum step frequency (1.67 Hz according to Nao documentation). | |
double | m_pathNextTargetDistance |
Cumulative Distance to next Velocity Target from current robot pose along path. | |
double | m_pathStartMaxDistance |
Max distance to start of path from robot pose to accept path (safety) | |
tf::Transform | m_prevRelTarget |
previous rel.target (used to prevent oscillations) | |
ros::Subscriber | m_simpleGoalSub |
ros::Subscriber | m_simplePathSub |
double | m_stepFactor |
Fraction of the maximum step frequency. | |
double | m_stepFreq |
Step frequency (should be set to the same value as in nao_walker). | |
bool | m_straight |
True = robot has a straight path in front of it with minimum length m_straightThreshold. | |
double | m_straightMaxPathDev |
Maximum angle deviation of the path off a straight (i.e. a curve in the path) until it is not recognized as such anymore. | |
double | m_straightMaxRobotDev |
Maximum XY deviation of the robot off a straight until it is not recognized as such anymore. | |
double | m_straightMaxRobotYaw |
Maximum angle of the robot off a straight until it is not recognized as such anymore. | |
double | m_straightOffset |
Length of the targetPose offset if a straight is recognized. | |
double | m_straightThreshold |
Minimum length of a straigth that is to be recognized as such. | |
double | m_targetAngThres |
Maximum angular deviation to reach target orientation. | |
double | m_targetDistThres |
Maximum linear deviation to reach target point. | |
ros::Publisher | m_targetPub |
tf::TransformListener | m_tfListener |
double | m_thresholdDampXY |
Slow down when distance to goal is smaller than this value [in meters]. | |
double | m_thresholdDampYaw |
Slow down rotation when angular deviation from goal pose is smaller than this value [in radians]. | |
double | m_thresholdFar |
double | m_thresholdRotate |
ros::ServiceClient | m_uninhibitWalkClient |
bool | m_useFootContactProtection |
True = abort walk when robot is lifted off the ground. | |
bool | m_useVelocityController |
True = use simple velocity controller, false = use Aldebaran's walkTo controller. | |
geometry_msgs::Twist | m_velocity |
last published velocity (used to prevent oscillations) | |
ros::Publisher | m_velPub |
ros::Publisher | m_visPub |
actionlib::SimpleActionServer < move_base_msgs::MoveBaseAction > | m_walkGoalServer |
actionlib::SimpleActionServer < naoqi_bridge_msgs::FollowPathAction > | m_walkPathServer |
double | m_waypointAngThres |
Maximum angular deviation to switch to next waypoint on path. | |
double | m_waypointDistThres |
Maximum linear deviation to switch to next waypoint on path. | |
ros::NodeHandle | nh |
ros::NodeHandle | privateNh |
Definition at line 48 of file nao_path_follower.cpp.
Definition at line 116 of file nao_path_follower.cpp.
Definition at line 193 of file nao_path_follower.cpp.
void PathFollower::footContactCallback | ( | const std_msgs::BoolConstPtr & | contact | ) | [private] |
Definition at line 228 of file nao_path_follower.cpp.
bool PathFollower::getNextTarget | ( | const nav_msgs::Path & | path, |
const tf::Pose & | robotPose, | ||
const std::vector< geometry_msgs::PoseStamped >::const_iterator & | currentPathPoseIt, | ||
tf::Stamped< tf::Transform > & | targetPose, | ||
std::vector< geometry_msgs::PoseStamped >::const_iterator & | targetPathPoseIt | ||
) | [private] |
Definition at line 345 of file nao_path_follower.cpp.
bool PathFollower::getRobotPose | ( | tf::StampedTransform & | globalToBase, |
const std::string & | global_frame_id | ||
) |
Definition at line 257 of file nao_path_follower.cpp.
void PathFollower::goalActionCB | ( | const move_base_msgs::MoveBaseGoalConstPtr & | goal | ) |
Definition at line 687 of file nao_path_follower.cpp.
void PathFollower::goalCB | ( | const geometry_msgs::PoseStampedConstPtr & | goal | ) |
Definition at line 211 of file nao_path_follower.cpp.
void PathFollower::inhibitJoystick | ( | ) | [private] |
Definition at line 232 of file nao_path_follower.cpp.
void PathFollower::pathActionCB | ( | const naoqi_bridge_msgs::FollowPathGoalConstPtr & | goal | ) |
Definition at line 469 of file nao_path_follower.cpp.
void PathFollower::pathCB | ( | const nav_msgs::PathConstPtr & | goal | ) |
Definition at line 219 of file nao_path_follower.cpp.
void PathFollower::publishTargetPoseVis | ( | const tf::Stamped< tf::Pose > & | targetPose | ) | [private] |
Publishes the target pose as a geometry_msgs/Pose for visualization.
targetPose | Target pose to publish. |
Definition at line 205 of file nao_path_follower.cpp.
void PathFollower::setVelocity | ( | const tf::Transform & | relTarget | ) | [private] |
Velocity controller.
relTarget | Target pose relative to the current robot pose. |
Strategy:
distance | angle | strategy ----------------------------------------------------------------- > m_thresholdFar | > m_thresholdRotate | rotate on the spot towards the target > m_thresholdFar | < m_thresholdRotate | walk towards the target, orient towards the target > m_targetDistThres | | walk towards the target, orient towards final yaw angle < m_targetDistThres | > m_targetAngThres | rotate on the spot towards final yaw angle < m_targetDistThres | < m_targetAngThres | stop
Definition at line 846 of file nao_path_follower.cpp.
void PathFollower::stopWalk | ( | ) | [private] |
Stop walk and uninhibit joystick.
Definition at line 797 of file nao_path_follower.cpp.
ros::Publisher PathFollower::m_actionGoalPub [private] |
Definition at line 70 of file nao_path_follower.cpp.
ros::Publisher PathFollower::m_actionPathPub [private] |
Definition at line 70 of file nao_path_follower.cpp.
std::string PathFollower::m_baseFrameId [private] |
Base frame of the robot.
Definition at line 80 of file nao_path_follower.cpp.
double PathFollower::m_controllerFreq [private] |
Desired frequency for controller loop.
Definition at line 81 of file nao_path_follower.cpp.
bool PathFollower::m_footContact [private] |
True = robot currently has foot contact with the ground.
Definition at line 88 of file nao_path_follower.cpp.
Definition at line 75 of file nao_path_follower.cpp.
Definition at line 74 of file nao_path_follower.cpp.
bool PathFollower::m_isJoystickInhibited [private] |
True = joystick walk commands are inhibited.
Definition at line 86 of file nao_path_follower.cpp.
const double PathFollower::m_maxStepFreq [private] |
Maximum step frequency (2.38 Hz according to Nao documentation).
Definition at line 100 of file nao_path_follower.cpp.
double PathFollower::m_maxVelFractionX [private] |
Maximum fraction of forward velocity for velocity controller, between 0.0 and 1.0.
Definition at line 92 of file nao_path_follower.cpp.
double PathFollower::m_maxVelFractionY [private] |
Maximum fraction of sideways velocity for velocity controller, between 0.0 and 1.0.
Definition at line 93 of file nao_path_follower.cpp.
double PathFollower::m_maxVelFractionYaw [private] |
Maximum fraction of rotational velocity for velocity controller, between 0.0 and 1.0.
Definition at line 94 of file nao_path_follower.cpp.
const double PathFollower::m_maxVelXY [private] |
Maximum translational velocity (0.0952 m/s according to Nao documentation).
Definition at line 97 of file nao_path_follower.cpp.
const double PathFollower::m_maxVelYaw [private] |
Maximum rotational velocity (47.6 deg/s according to Nao documentation).
Definition at line 98 of file nao_path_follower.cpp.
const double PathFollower::m_minStepFreq [private] |
Minimum step frequency (1.67 Hz according to Nao documentation).
Definition at line 99 of file nao_path_follower.cpp.
double PathFollower::m_pathNextTargetDistance [private] |
Cumulative Distance to next Velocity Target from current robot pose along path.
Definition at line 106 of file nao_path_follower.cpp.
double PathFollower::m_pathStartMaxDistance [private] |
Max distance to start of path from robot pose to accept path (safety)
Definition at line 107 of file nao_path_follower.cpp.
tf::Transform PathFollower::m_prevRelTarget [private] |
previous rel.target (used to prevent oscillations)
Definition at line 77 of file nao_path_follower.cpp.
ros::Subscriber PathFollower::m_simpleGoalSub [private] |
Definition at line 69 of file nao_path_follower.cpp.
ros::Subscriber PathFollower::m_simplePathSub [private] |
Definition at line 69 of file nao_path_follower.cpp.
double PathFollower::m_stepFactor [private] |
Fraction of the maximum step frequency.
Definition at line 96 of file nao_path_follower.cpp.
double PathFollower::m_stepFreq [private] |
Step frequency (should be set to the same value as in nao_walker).
Definition at line 95 of file nao_path_follower.cpp.
bool PathFollower::m_straight [private] |
True = robot has a straight path in front of it with minimum length m_straightThreshold.
Definition at line 113 of file nao_path_follower.cpp.
double PathFollower::m_straightMaxPathDev [private] |
Maximum angle deviation of the path off a straight (i.e. a curve in the path) until it is not recognized as such anymore.
Definition at line 110 of file nao_path_follower.cpp.
double PathFollower::m_straightMaxRobotDev [private] |
Maximum XY deviation of the robot off a straight until it is not recognized as such anymore.
Definition at line 108 of file nao_path_follower.cpp.
double PathFollower::m_straightMaxRobotYaw [private] |
Maximum angle of the robot off a straight until it is not recognized as such anymore.
Definition at line 109 of file nao_path_follower.cpp.
double PathFollower::m_straightOffset [private] |
Length of the targetPose offset if a straight is recognized.
Definition at line 112 of file nao_path_follower.cpp.
double PathFollower::m_straightThreshold [private] |
Minimum length of a straigth that is to be recognized as such.
Definition at line 111 of file nao_path_follower.cpp.
double PathFollower::m_targetAngThres [private] |
Maximum angular deviation to reach target orientation.
Definition at line 83 of file nao_path_follower.cpp.
double PathFollower::m_targetDistThres [private] |
Maximum linear deviation to reach target point.
Definition at line 82 of file nao_path_follower.cpp.
ros::Publisher PathFollower::m_targetPub [private] |
Definition at line 70 of file nao_path_follower.cpp.
Definition at line 71 of file nao_path_follower.cpp.
double PathFollower::m_thresholdDampXY [private] |
Slow down when distance to goal is smaller than this value [in meters].
Definition at line 103 of file nao_path_follower.cpp.
double PathFollower::m_thresholdDampYaw [private] |
Slow down rotation when angular deviation from goal pose is smaller than this value [in radians].
Definition at line 104 of file nao_path_follower.cpp.
double PathFollower::m_thresholdFar [private] |
Definition at line 101 of file nao_path_follower.cpp.
double PathFollower::m_thresholdRotate [private] |
Definition at line 102 of file nao_path_follower.cpp.
Definition at line 74 of file nao_path_follower.cpp.
bool PathFollower::m_useFootContactProtection [private] |
True = abort walk when robot is lifted off the ground.
Definition at line 87 of file nao_path_follower.cpp.
bool PathFollower::m_useVelocityController [private] |
True = use simple velocity controller, false = use Aldebaran's walkTo controller.
Definition at line 91 of file nao_path_follower.cpp.
geometry_msgs::Twist PathFollower::m_velocity [private] |
last published velocity (used to prevent oscillations)
Definition at line 78 of file nao_path_follower.cpp.
ros::Publisher PathFollower::m_velPub [private] |
Definition at line 70 of file nao_path_follower.cpp.
ros::Publisher PathFollower::m_visPub [private] |
Definition at line 70 of file nao_path_follower.cpp.
actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> PathFollower::m_walkGoalServer [private] |
Definition at line 72 of file nao_path_follower.cpp.
actionlib::SimpleActionServer<naoqi_bridge_msgs::FollowPathAction> PathFollower::m_walkPathServer [private] |
Definition at line 73 of file nao_path_follower.cpp.
double PathFollower::m_waypointAngThres [private] |
Maximum angular deviation to switch to next waypoint on path.
Definition at line 85 of file nao_path_follower.cpp.
double PathFollower::m_waypointDistThres [private] |
Maximum linear deviation to switch to next waypoint on path.
Definition at line 84 of file nao_path_follower.cpp.
ros::NodeHandle PathFollower::nh [private] |
Definition at line 67 of file nao_path_follower.cpp.
ros::NodeHandle PathFollower::privateNh [private] |
Definition at line 68 of file nao_path_follower.cpp.