MILDRobotModel implements a model of pan tilt unit robot. More...
#include <MILDRobotModel.hpp>
Public Member Functions | |
geometry_msgs::Pose | calculateCameraPose (const RobotStatePtr &sourceRobotState) |
Uses a given RobotState to calculate the camera frame. More... | |
float | getBase_RotationalMovementCosts (const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState) |
float | getBase_TranslationalMovementCosts (const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState) |
Calculates the movement costs from sourceRobotState to targetRobotState. Returns -1 if pose is not reachable. More... | |
geometry_msgs::Pose | getCameraPose () |
float | getDistance (const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition) |
nav_msgs::Path | getNavigationPath (const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition, double sourceRotationBase, double targetRotationBase) |
nav_msgs::Path | getNavigationPath (const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition) |
float | getPTU_PanMovementCosts (const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState) |
float | getPTU_TiltMovementCosts (const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState) |
geometry_msgs::Pose | getRobotPose () |
bool | isPoseReachable (const SimpleVector3 &position, const SimpleQuaternion &orientation) |
bool | isPositionAllowed (const geometry_msgs::Point &position) |
bool | isPositionReachable (const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition) |
MILDRobotModel () | |
constructor of the MILDRobotModel More... | |
void | setPanAngleLimits (float minAngleDegrees, float maxAngleDegrees) |
sets the angle limits of the pan angle. More... | |
void | setRotationAngleLimits (float minAngleDegrees, float maxAngleDegrees) |
sets the angle limits of the rotation angle. More... | |
void | setTiltAngleLimits (float minAngleDegrees, float maxAngleDegrees) |
sets the angle limits of the tilt angle. More... | |
virtual | ~MILDRobotModel () |
destructor of the MILDRobotModel More... | |
Public Member Functions inherited from robot_model_services::RobotModel | |
RobotStatePtr | calculateRobotState (const robot_model_services::SimpleVector3 &position, const robot_model_services::SimpleQuaternion &orientation) |
calculates the target robot state by assuming the saved current state of the roboter as source state More... | |
virtual RobotStatePtr | calculateRobotState (const RobotStatePtr &sourceRobotState, const SimpleVector3 &position, const SimpleQuaternion &orientation)=0 |
calculates the target robot state More... | |
float | getBase_RotationalMovementCosts (const RobotStatePtr &targetRobotState) |
float | getBase_TranslationalMovementCosts (const RobotStatePtr &targetRobotState) |
RobotStatePtr | getCurrentRobotState () |
float | getPTU_PanMovementCosts (const RobotStatePtr &targetRobotState) |
float | getPTU_TiltMovementCosts (const RobotStatePtr &targetRobotState) |
RobotModel () | |
the constructor of the RobotModel object More... | |
void | setCurrentRobotState (const RobotStatePtr ¤tRobotState) |
virtual | ~RobotModel () |
the destructor of the RobotModel object More... | |
Protected Attributes | |
tf::TransformListener | listener |
DebugHelperPtr | mDebugHelperPtr |
std::string | mFrameName_map |
std::string | mFrameName_mild_base |
std::string | mFrameName_mild_camera_left |
std::string | mFrameName_mild_camera_mount_link |
std::string | mFrameName_mild_camera_right |
std::string | mFrameName_mild_ptu_base_link |
std::string | mFrameName_mild_ptu_pan_link |
std::string | mFrameName_mild_ptu_pan_link_rotated |
std::string | mFrameName_mild_ptu_tilt_link |
std::string | mFrameName_mild_ptu_tilt_link_rotated |
MapHelperPtr | mMapHelperPtr |
float | mOmegaPan |
float | mOmegaRot |
float | mOmegaTilt |
boost::tuple< float, float > | mPanLimits |
boost::tuple< float, float > | mRotationLimits |
float | mSigma |
boost::tuple< float, float > | mTiltLimits |
ros::NodeHandle | n |
ros::ServiceClient | navigationCostClient |
float | speedFactorBaseMove |
float | speedFactorBaseRot |
float | speedFactorPTU |
float | tolerance |
bool | useGlobalPlanner |
Private Member Functions | |
void | initMapHelper () |
Init MapHelper if it is not initialized yet. More... | |
MILDRobotModel implements a model of pan tilt unit robot.
Definition at line 39 of file MILDRobotModel.hpp.
robot_model_services::MILDRobotModel::MILDRobotModel | ( | ) |
constructor of the MILDRobotModel
Definition at line 44 of file MILDRobotModel.cpp.
|
virtual |
destructor of the MILDRobotModel
Definition at line 127 of file MILDRobotModel.cpp.
|
virtual |
Uses a given RobotState to calculate the camera frame.
Implements robot_model_services::RobotModel.
Definition at line 400 of file MILDRobotModel.cpp.
|
virtual |
Implements robot_model_services::RobotModel.
Definition at line 277 of file MILDRobotModel.cpp.
|
virtual |
Calculates the movement costs from sourceRobotState to targetRobotState. Returns -1 if pose is not reachable.
Implements robot_model_services::RobotModel.
Definition at line 224 of file MILDRobotModel.cpp.
|
virtual |
Implements robot_model_services::RobotModel.
Definition at line 141 of file MILDRobotModel.cpp.
|
virtual |
Implements robot_model_services::RobotModel.
Definition at line 305 of file MILDRobotModel.cpp.
|
virtual |
Client used for communication with the global_planner to calculate movement costs
Implements robot_model_services::RobotModel.
Definition at line 358 of file MILDRobotModel.cpp.
|
virtual |
Implements robot_model_services::RobotModel.
Definition at line 353 of file MILDRobotModel.cpp.
|
virtual |
Implements robot_model_services::RobotModel.
Definition at line 244 of file MILDRobotModel.cpp.
|
virtual |
Implements robot_model_services::RobotModel.
Definition at line 261 of file MILDRobotModel.cpp.
|
virtual |
Implements robot_model_services::RobotModel.
Definition at line 129 of file MILDRobotModel.cpp.
|
private |
Init MapHelper if it is not initialized yet.
Definition at line 181 of file MILDRobotModel.cpp.
|
virtual |
orientation | the orientation to reach |
Implements robot_model_services::RobotModel.
Definition at line 195 of file MILDRobotModel.cpp.
bool robot_model_services::MILDRobotModel::isPositionAllowed | ( | const geometry_msgs::Point & | position | ) |
Definition at line 171 of file MILDRobotModel.cpp.
|
virtual |
Implements robot_model_services::RobotModel.
Definition at line 203 of file MILDRobotModel.cpp.
void robot_model_services::MILDRobotModel::setPanAngleLimits | ( | float | minAngleDegrees, |
float | maxAngleDegrees | ||
) |
sets the angle limits of the pan angle.
minAngleDegrees | the minimum angle in degrees |
maxAngleDegrees | the maximum angle in degrees |
Definition at line 153 of file MILDRobotModel.cpp.
void robot_model_services::MILDRobotModel::setRotationAngleLimits | ( | float | minAngleDegrees, |
float | maxAngleDegrees | ||
) |
sets the angle limits of the rotation angle.
minAngleDegrees | the minimum angle in degrees |
maxAngleDegrees | the maximum angle in degrees |
Definition at line 165 of file MILDRobotModel.cpp.
void robot_model_services::MILDRobotModel::setTiltAngleLimits | ( | float | minAngleDegrees, |
float | maxAngleDegrees | ||
) |
sets the angle limits of the tilt angle.
minAngleDegrees | the minimum angle in degrees |
maxAngleDegrees | the maximum angle in degrees |
Definition at line 159 of file MILDRobotModel.cpp.
|
protected |
Definition at line 55 of file MILDRobotModel.hpp.
|
protected |
Definition at line 43 of file MILDRobotModel.hpp.
|
protected |
contains the definitions of the robot's frame names
Definition at line 63 of file MILDRobotModel.hpp.
|
protected |
Definition at line 64 of file MILDRobotModel.hpp.
|
protected |
Definition at line 71 of file MILDRobotModel.hpp.
|
protected |
Definition at line 70 of file MILDRobotModel.hpp.
|
protected |
Definition at line 72 of file MILDRobotModel.hpp.
|
protected |
Definition at line 65 of file MILDRobotModel.hpp.
|
protected |
Definition at line 66 of file MILDRobotModel.hpp.
|
protected |
Definition at line 67 of file MILDRobotModel.hpp.
|
protected |
Definition at line 68 of file MILDRobotModel.hpp.
|
protected |
Definition at line 69 of file MILDRobotModel.hpp.
|
protected |
Definition at line 56 of file MILDRobotModel.hpp.
|
protected |
Definition at line 46 of file MILDRobotModel.hpp.
|
protected |
Definition at line 48 of file MILDRobotModel.hpp.
|
protected |
Definition at line 47 of file MILDRobotModel.hpp.
|
protected |
contains the lower and upper limit of pan
Definition at line 77 of file MILDRobotModel.hpp.
|
protected |
contains the lower and upper limit of rotation
Definition at line 87 of file MILDRobotModel.hpp.
|
protected |
Definition at line 53 of file MILDRobotModel.hpp.
|
protected |
contains the lower and upper limit of tilt
Definition at line 82 of file MILDRobotModel.hpp.
|
protected |
Definition at line 58 of file MILDRobotModel.hpp.
|
protected |
Client used for communication with the global_planner to calculate movement costs
Definition at line 92 of file MILDRobotModel.hpp.
|
protected |
Definition at line 50 of file MILDRobotModel.hpp.
|
protected |
Definition at line 51 of file MILDRobotModel.hpp.
|
protected |
Definition at line 49 of file MILDRobotModel.hpp.
|
protected |
Definition at line 52 of file MILDRobotModel.hpp.
|
protected |
Definition at line 54 of file MILDRobotModel.hpp.