$search
#include <move_to_table_node.h>
Public Member Functions | |
void | addMarkerForFinalPose (geometry_msgs::Pose finalPose) |
adds a marker for showing the final target | |
bool | doIntersect (float line) |
finds whether there is an intersection between the line through the robot pose and table centroid and the boundies of the table | |
Eigen::Quaternionf | faceTable (geometry_msgs::Pose finalPose) |
sets the orientation of the final target | |
geometry_msgs::Pose | findIntersectionPoint () |
finds the intersection between the line through the robot pose and table centroid and the boundies of the table | |
geometry_msgs::Pose | findSafeTargetPoint () |
finds a safe position in the vicinity of the table as the target | |
MoveToTableNode () | |
bool | moveToTableService (cob_3d_mapping_msgs::MoveToTable::Request &req, cob_3d_mapping_msgs::MoveToTable::Response &res) |
service callback for MoveToTable service | |
geometry_msgs::Pose | transformToTableCoordinateSystem (cob_3d_mapping_msgs::Table &table, geometry_msgs::Pose &Pose) |
transforms a point to table coordinate system | |
~MoveToTableNode () | |
Public Attributes | |
ros::NodeHandle | n_ |
geometry_msgs::Pose | robotPose_ |
geometry_msgs::Pose | robotPoseInTableCoordinateSys_ |
float | safe_dist_ |
cob_3d_mapping_msgs::Table | table_ |
Eigen::Matrix4f | transformToTableCoordinateSys_ |
Protected Attributes | |
ros::ServiceServer | move_to_table_server_ |
ros::Publisher | navigation_goal_pub_ |
boost::shared_ptr < interactive_markers::InteractiveMarkerServer > | table_im_server_ |
Definition at line 81 of file move_to_table_node.h.
MoveToTableNode::MoveToTableNode | ( | ) |
Definition at line 63 of file move_to_table_node.cpp.
MoveToTableNode::~MoveToTableNode | ( | ) | [inline] |
void
Definition at line 87 of file move_to_table_node.h.
void MoveToTableNode::addMarkerForFinalPose | ( | geometry_msgs::Pose | finalPose | ) |
adds a marker for showing the final target
Definition at line 271 of file move_to_table_node.cpp.
bool MoveToTableNode::doIntersect | ( | float | line | ) |
finds whether there is an intersection between the line through the robot pose and table centroid and the boundies of the table
[in] | line | the line which needs to be checked for the intersection |
Definition at line 132 of file move_to_table_node.cpp.
Eigen::Quaternionf MoveToTableNode::faceTable | ( | geometry_msgs::Pose | finalPose | ) |
sets the orientation of the final target
[in] | finalPose | target position |
[out] | quaternion | representing the orientation of final pose |
Definition at line 70 of file move_to_table_node.cpp.
geometry_msgs::Pose MoveToTableNode::findIntersectionPoint | ( | ) |
finds the intersection between the line through the robot pose and table centroid and the boundies of the table
[out] | intersection | position |
Definition at line 197 of file move_to_table_node.cpp.
geometry_msgs::Pose MoveToTableNode::findSafeTargetPoint | ( | ) |
finds a safe position in the vicinity of the table as the target
[out] | safe | target position |
Definition at line 236 of file move_to_table_node.cpp.
bool MoveToTableNode::moveToTableService | ( | cob_3d_mapping_msgs::MoveToTable::Request & | req, | |
cob_3d_mapping_msgs::MoveToTable::Response & | res | |||
) |
service callback for MoveToTable service
[in] | req | request to move to table |
[in] | res | empty response |
Definition at line 325 of file move_to_table_node.cpp.
geometry_msgs::Pose MoveToTableNode::transformToTableCoordinateSystem | ( | cob_3d_mapping_msgs::Table & | table, | |
geometry_msgs::Pose & | Pose | |||
) |
transforms a point to table coordinate system
[in] | table | table msg |
[in] | pose | the pose which needs to be transformed ro table coordinate system |
[out] | transformed | point |
Definition at line 94 of file move_to_table_node.cpp.
Definition at line 153 of file move_to_table_node.h.
Definition at line 144 of file move_to_table_node.h.
ros::Publisher MoveToTableNode::navigation_goal_pub_ [protected] |
Definition at line 155 of file move_to_table_node.h.
Definition at line 147 of file move_to_table_node.h.
Definition at line 146 of file move_to_table_node.h.
Definition at line 150 of file move_to_table_node.h.
Definition at line 145 of file move_to_table_node.h.
boost::shared_ptr<interactive_markers::InteractiveMarkerServer> MoveToTableNode::table_im_server_ [protected] |
Definition at line 154 of file move_to_table_node.h.
Eigen::Matrix4f MoveToTableNode::transformToTableCoordinateSys_ |
Definition at line 149 of file move_to_table_node.h.