This state creator adds the current robot pose to the state. More...

#include <stateCreatorRobotPose.h>

Inheritance diagram for tidyup_actions::StateCreatorRobotPose:

Public Member Functions
virtual bool	fillState (SymbolicState &state)
virtual void	initialize (const std::deque< std::string > &arguments)
	Initialize the state creator parameters.
	StateCreatorRobotPose ()
	~StateCreatorRobotPose ()
Protected Member Functions
bool	extractPoseStamped (const SymbolicState &state, const string &object, geometry_msgs::PoseStamped &pose) const
	Extract a PoseStamped for object from state.
sensor_msgs::JointState	getArmSideJointState () const
	Produces arm state for arms at side.
std_msgs::ColorRGBA	getLocationColor (const SymbolicState &state, const string &location) const
visualization_msgs::MarkerArray	getLocationMarkers (const SymbolicState &state, const string &location, const string &ns, int id, bool useMeshes) const
	Create a marker for location.
void	publishLocationsAsMarkers (const SymbolicState &state)
Protected Attributes
std::string	_atPredicate
	the name of the "at" predicate (e.g. at-base)
double	_goalToleranceXY
double	_goalToleranceYaw
std::string	_locationType
	the type of location objects that a robot might be "at"
ros::Publisher	_markerPub
std::string	_robotPoseObject
	the name of the robot pose's object (e.g. robot_pose, or l0)
std::string	_robotPoseType
	the type of the _robotPoseObject - required if _robotPoseObject.
RobotPoseVisualization	_robotPoseVis
tf::TransformListener	_tf
Static Protected Attributes
static const bool	s_PublishLocationsAsMarkers = true
static const bool	s_PublishMeshMarkers = true

Detailed Description

This state creator adds the current robot pose to the state.

The current pose is estimated via tf as the transform from /map to /base_link. A stamped pose in the state is represented by the fluents: x y z qx qy qz qw as well as timestamp and frame-id

The state creator serves two purposes: 1. The fluents for the robot_pose are set to the real values 2. An at-style predicate of the form (at location) is set.

1. If _robotPoseObject is not empty the x,y,z,etc. fluents will be filled accordingly. 2.a If the _atPredicate is not empty, the at predicate will be set to the current robot pose. 2.b If a _locationType is given it is checked if the current robot pose is one of the poses in the objects of _locationType and thus _atPredicate is possibly set to a location instead of the current pose. If a robot is "at" a location is determined by the _goalToleranceXY and _goalToleranceYaw.

Definition at line 29 of file stateCreatorRobotPose.h.

Constructor & Destructor Documentation

tidyup_actions::StateCreatorRobotPose::StateCreatorRobotPose ( )

tidyup_actions::StateCreatorRobotPose::~StateCreatorRobotPose ( )

Member Function Documentation

bool tidyup_actions::StateCreatorRobotPose::extractPoseStamped	(	const SymbolicState &	state,
		const string &	object,
		geometry_msgs::PoseStamped &	pose
	)		const `[protected]`

Extract a PoseStamped for object from state.

The fluents that are queried are: x,y,z, qx,qy,qz,qw, frame-id, timestamp

Returns:: true if all fluents were available

virtual bool tidyup_actions::StateCreatorRobotPose::fillState ( SymbolicState & state ) [virtual]

Implements continual_planning_executive::StateCreator.

sensor_msgs::JointState tidyup_actions::StateCreatorRobotPose::getArmSideJointState ( ) const [protected]

Produces arm state for arms at side.

std_msgs::ColorRGBA tidyup_actions::StateCreatorRobotPose::getLocationColor	(	const SymbolicState &	state,
		const string &	location
	)		const `[protected]`

Get the color that location should have based on the fact that it is the robot location or another and if the robot is actually at that location.

visualization_msgs::MarkerArray tidyup_actions::StateCreatorRobotPose::getLocationMarkers	(	const SymbolicState &	state,
		const string &	location,
		const string &	ns,
		int	id,
		bool	useMeshes
	)		const `[protected]`