Class for creating, editing, and saving planning scenes. More...

#include <move_arm_utils.h>

Inheritance diagram for planning_scene_utils::PlanningSceneEditor:

Classes
struct	IKController
	Struct containing the start and end 6DOF controllers for a specific motion plan request. More...
struct	SelectableObject
	Struct containing an interactive marker for 6DOF control, and another for selection. More...
struct	StateRegistry
	convenience class for keeping track of KinematicStates. This must be done, because if not all kinematic states are deleted before SendPlanningScene() is called, the environment server is liable to hang. More...
Public Types
enum	GeneratedShape { Box, Cylinder, Sphere }
	These kinds of shapes can be created by the editor. More...
Public Member Functions
void	armHasStoppedMoving ()
	Called when the robot actually stops moving, following execution of the trajectory.
void	attachedCollisionObjectInteractiveCallback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void	collisionObjectMovementCallback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
	called when a collision object is moved in rviz.
void	collisionObjectSelectionCallback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
	called when a collision object is selected in rviz.
void	controllerDoneCallback (const actionlib::SimpleClientGoalState &state, const control_msgs::FollowJointTrajectoryResultConstPtr &result)
	Called when the robot monitor detects that the robot has stopped following a trajectory.
std::string	createCollisionObject (const std::string &name, geometry_msgs::Pose pose, GeneratedShape shape, float scaleX, float scaleY, float scaleZ, std_msgs::ColorRGBA color)
	creates an entirely new collision object and places it in the environment.
void	createIKController (MotionPlanRequestData &data, PositionType type, bool rePose=true)
	creates a 6DOF control over the end effector of either the start or goal position of the given request.
void	createIkControllersFromMotionPlanRequest (MotionPlanRequestData &data, bool rePose=true)
	creates both the start and goal 6DOF controls for the given request, but only if those are visible and editable.
void	createJointMarkers (MotionPlanRequestData &data, planning_scene_utils::PositionType position)
	creates 1DOF controls for each of the joints of the given request and its start and end positions.
std::string	createMeshObject (const std::string &name, geometry_msgs::Pose pose, const std::string &filename, const tf::Vector3 &scale, std_msgs::ColorRGBA color)
void	createMotionPlanRequest (const planning_models::KinematicState &start_state, const planning_models::KinematicState &end_state, const std::string &group_name, const std::string &end_effector_name, const unsigned int &planning_scene_name, const bool fromRobotState, unsigned int &motionPlan_id_Out)
	Creates an entirely new motion plan request with the given parameters.
std::string	createNewPlanningScene ()
	creates an entirely new, empty planning scene.
void	deleteCollisionObject (std::string &name)
	Removes the collision object with the specified name from the world.
void	deleteJointMarkers (MotionPlanRequestData &data, PositionType type)
	erases all the interactive 1DOF markers on the joints of the given request.
void	deleteKinematicStates ()
	All kinematic states in the editor are kept track of, and must be deleted with this function before the planning scene can be sent to the environment server. Otherwise, the editor will hang.
void	deleteMotionPlanRequest (const unsigned int &id, std::vector< unsigned int > &erased_trajectories)
	erases the given motion plan request and all its associated trajectories.
void	deleteTrajectory (unsigned int mpr_id, unsigned int traj_id)
	erases the given trajectory from the trajectory map.
void	executeTrajectory (const std::string &mpr_name, const std::string &traj_name)
	Creates orientation constraints from a given robot state.
void	executeTrajectory (TrajectoryData &data)
	if real robot data is being used, this can be used to send a trajectory to the robot for execution.
bool	filterTrajectory (MotionPlanRequestData &requestData, TrajectoryData &trajectory, unsigned int &filter_id)
	Calls the trajectory filter service on the given trajectory.
std::string	generateNewCollisionObjectId ()
	generates a unique collision object id.
unsigned int	generateNewPlanningSceneId ()
	generates a unique planning scene id.
bool	getAllAssociatedMotionPlanRequests (const unsigned int id, std::vector< unsigned int > &ids, std::vector< std::string > &stages, std::vector< arm_navigation_msgs::MotionPlanRequest > &requests)
	loads motion plan requests associated with the given timestamp from the warehouse.
bool	getAllAssociatedPausedStates (const unsigned int id, std::vector< ros::Time > &paused_times)
	loads all paused states from the warehouse associated with the given time stamp.
bool	getAllAssociatedTrajectorySources (const unsigned int planning_id, const unsigned int mpr_id, std::vector< unsigned int > &trajectory_ids, std::vector< std::string > &trajectory_sources)
	loads all trajectory sources from the warehouse associated with the given time stamp.
bool	getAllPlanningSceneTimes (std::vector< ros::Time > &planning_scene_times, std::vector< unsigned int > &planning_scene_ids)
	loads all planning scene times from the warehouse.
void	getAllRobotStampedTransforms (const planning_models::KinematicState &state, std::vector< geometry_msgs::TransformStamped > &trans_vector, const ros::Time &stamp)
	gets TF data for the given kinematic state.
planning_environment::CollisionModels *	getCollisionModel ()
move_arm_warehouse::MoveArmWarehouseLoggerReader *	getLoggerReader ()
void	getMotionPlanningMarkers (visualization_msgs::MarkerArray &arr)
	Fills the given array with mesh markers associated with all motion plan requests.
bool	getMotionPlanRequest (const ros::Time &time, const std::string &stage, arm_navigation_msgs::MotionPlanRequest &mpr, std::string &id, std::string &planning_scene_id)
	loads a specific motion plan request from the warehouse.
bool	getPausedState (const unsigned int id, const ros::Time &paused_time, head_monitor_msgs::HeadMonitorFeedback &paused_state)
	loads a specific paused state from the warehouse
bool	getPlanningSceneOutcomes (const unsigned int id, std::vector< std::string > &pipeline_stages, std::vector< arm_navigation_msgs::ArmNavigationErrorCodes > &error_codes, std::map< std::string, arm_navigation_msgs::ArmNavigationErrorCodes > &error_map)
	loads all the error codes associated with a particular planning scene from the warehouse.
planning_models::KinematicState *	getRobotState ()
void	getTrajectoryMarkers (visualization_msgs::MarkerArray &arr)
	Fills the given array with mesh markers associated with all trajectories.
bool	hasTrajectory (const std::string &mpr_name, const std::string &traj_name) const
void	IKControllerCallback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
	Called when a 6DOF interactive marker is altered in RVIZ.
void	initMotionPlanRequestData (const unsigned int &planning_scene_id, const std::vector< unsigned int > &ids, const std::vector< std::string > &stages, const std::vector< arm_navigation_msgs::MotionPlanRequest > &requests)
	fills the motion_plan_map with new motion plan requests initialized with warehouse data.
void	JointControllerCallback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
	Called when a 1DOF joint marker is altered in RVIZ.
void	jointStateCallback (const sensor_msgs::JointStateConstPtr &joint_state)
	Called when the robot monitor detects a change in the robot state.
void	jointTrajectoryControllerStateCallback (const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &joint_state)
	Called when the arm controller has a state update.
void	loadAllWarehouseData ()
	gets all the motion plan requests, trajectories, and planning scenes from the warehouse.
bool	loadPlanningScene (const ros::Time &time, const unsigned int id)
	loads a particular planning scene from the warehouse
void	lockScene ()
void	makeInteractive1DOFRotationMarker (tf::Transform transform, tf::Vector3 axis, std::string name, std::string description, float scale=1.0f, float angle=0.0f)
	creates an interactive marker for rotation joint control.
void	makeInteractive1DOFTranslationMarker (tf::Transform transform, tf::Vector3 axis, std::string name, std::string description, float scale=1.0f, float value=0.0f)
	creates an interactive marker for prismatic joint control.
void	makeSelectableAttachedObjectFromPlanningScene (const arm_navigation_msgs::PlanningScene &scene, arm_navigation_msgs::AttachedCollisionObject &att)
virtual void	onPlanningSceneLoaded (int scene, int numScenes)
	pure virtual function that acts as a callback when a given scene was loaded. (Use case: things like loading bars)
	PlanningSceneEditor ()
	PlanningSceneEditor (PlanningSceneParameters &params)
bool	planToKinematicState (const planning_models::KinematicState &state, const std::string &group_name, const std::string &end_effector_name, unsigned int &trajectoryid_Out, unsigned int &planning_scene_id)
	invokes the planner service to plan from the current robot state to the given kinematic state.
bool	planToRequest (MotionPlanRequestData &data, unsigned int &trajectoryid_Out)
	invokes the planner to plan from the start position of the request to the goal position.
bool	planToRequest (const std::string &requestid, unsigned int &trajectoryid_Out)
	invokes the planner to plan from the start position of the request to the goal position.
bool	playTrajectory (MotionPlanRequestData &requestData, TrajectoryData &data)
	non-blocking call resetting the given trajectory and setting it to play and be visible.
void	printTrajectoryPoint (const std::vector< std::string > &joint_names, const std::vector< double > &joint_values)
	for debugging, prints the given trajectory point values.
void	randomlyPerturb (MotionPlanRequestData &mpr, PositionType type)
	sets the motion plan request start or goal to a set of random joint values. Avoids collisions
interactive_markers::MenuHandler::EntryHandle	registerMenuEntry (std::string menu, std::string entryName, interactive_markers::MenuHandler::FeedbackCallback &callback)
	creates an interactive marker menu with the given name, associating it with a callback function.
interactive_markers::MenuHandler::EntryHandle	registerSubMenuEntry (std::string menu, std::string name, std::string subMenu, interactive_markers::MenuHandler::FeedbackCallback &callback)
	registers a new menu entry as a sub menu of an exitsting interactive marker menu entry
void	savePlanningScene (PlanningSceneData &data, bool copy=false)
	Pushes the given planning scene to the warehouse with ros::WallTime::now() as its timestamp.
void	sendMarkers ()
	sends all stored mesh and sphere markers for collisions and links to rviz.
bool	sendPlanningScene (PlanningSceneData &data)
	sends a planning scene diff message to the environment server, updating the global planning scene.
void	sendTransformsAndClock ()
	sends all TF transforms and a wall clock time to ROS.
void	setCollisionModel (planning_environment::CollisionModels *model, bool shouldDelete=false)
void	setCurrentPlanningScene (std::string id, bool loadRequests=true, bool loadTrajectories=true)
	loads the given planning scene from the warehouse.
void	setIKControlsVisible (std::string id, PositionType type, bool visible)
	either shows or hides the 6DOF interactive markers associated with the given request.
void	setJointState (MotionPlanRequestData &data, PositionType position, std::string &jointName, tf::Transform value)
	attempts to set the state of the given joint in the given motion plan request so that it matches the given transform.
void	setLoggerReader (move_arm_warehouse::MoveArmWarehouseLoggerReader *loggerReader, bool shouldDelete=true)
void	setRobotState (planning_models::KinematicState *robot_state, bool shouldDelete=true)
bool	solveIKForEndEffectorPose (MotionPlanRequestData &mpr, PositionType type, bool coll_aware=true, double change_redundancy=0.0)
	invokes the inverse kinematics solver on the given motion plan requests' start or end state, setting the joint values of that state to the solution.
void	unlockScene ()
void	updateJointStates ()
	if robot data is not being used, publishes fake joint states of the current robot state to a robot state publisher node.
	~PlanningSceneEditor ()
Public Attributes
std::map< std::string, bool >	joint_clicked_map_
	Map of joint controls and whether they have been clicked by the user.
std::map< std::string, tf::Transform >	joint_prev_transform_map_
	Map of joint controls and their last transforms.
std::map< std::string, MotionPlanRequestData >	motion_plan_map_
	Map containing all motion plan requests, indexed by (unique) name.
std::map< std::string, PlanningSceneData >	planning_scene_map_
	Map containing all planning scenes, indexed by (unique) name.
std::map< std::string, std::map< std::string, TrajectoryData > >	trajectory_map_
	Map containing all trajectories, indexed by (unique) motion plan id name, and then by unique trajectory name.
Protected Types
enum	MonitorStatus { idle, Executing, WaitingForStop, Done }
	PlanningSceneEditor monitors robot state while "use_robot_data_" is true. When in idle mode, the monitor is not recording robot state into a trajectory. In Executing mode, the monitor records to a trajectory. In Done mode, the final trajectory is saved to the trajectory map. More...
Protected Member Functions
void	attachCollisionObject (const std::string &name, const std::string &link_name, const std::vector< std::string > &touch_links)
virtual void	attachObjectCallback (const std::string &name)=0
void	changeToAttached (const std::string &name)
void	createSelectableMarkerFromCollisionObject (arm_navigation_msgs::CollisionObject &object, std::string name, std::string description, std_msgs::ColorRGBA color, bool insert_selectable=true)
	Registers a collision object as a selectable marker.
virtual void	filterCallback (arm_navigation_msgs::ArmNavigationErrorCodes &errorCode)=0
	Virtual function called when the filter is invoked.
virtual void	planCallback (arm_navigation_msgs::ArmNavigationErrorCodes &errorCode)=0
	Virtual function called when the planner is invoked.
virtual void	selectedTrajectoryCurrentPointChanged (unsigned int new_current_point)
	Called when the selected trajectory of the selected motion plan updates its current point. Override to do something useful.
virtual void	updateState ()
	Pure virtual function called when a trajectory, motion plan request, or the robot's state is changed.
Protected Attributes
int	active_planner_index_
std::map< std::string, actionlib::SimpleActionClient < control_msgs::FollowJointTrajectoryAction > * >	arm_controller_map_
interactive_markers::MenuHandler::FeedbackCallback	attached_collision_object_feedback_ptr_
ros::Publisher	clock_publisher_
planning_environment::CollisionModels *	cm_
std::map< std::string, ros::ServiceClient * > *	collision_aware_ik_services_
std::string	collision_marker_state_
visualization_msgs::MarkerArray	collision_markers_
interactive_markers::MenuHandler::FeedbackCallback	collision_object_movement_feedback_ptr_
interactive_markers::MenuHandler::FeedbackCallback	collision_object_selection_feedback_ptr_
ros::ServiceClient	collision_proximity_planner_client_
std::string	current_planning_scene_name_
ros::ServiceClient	distance_aware_service_client_
ros::ServiceClient	distance_state_validity_service_client_
std::map< std::string, arm_navigation_msgs::ArmNavigationErrorCodes >	error_map_
ros::ServiceClient	gazebo_joint_state_client_
ros::ServiceClient	get_link_properties_client_
interactive_markers::MenuHandler::FeedbackCallback	ik_control_feedback_ptr_
std::map< std::string, IKController > *	ik_controllers_
interactive_markers::InteractiveMarkerServer *	interactive_marker_server_
std::map< std::string, ros::ServiceClient * > *	interpolated_ik_services_
interactive_markers::MenuHandler::FeedbackCallback	joint_control_feedback_ptr_
ros::Publisher	joint_state_publisher_
ros::Subscriber	joint_state_subscriber_
ros::Subscriber	l_arm_controller_state_subscriber_
std_msgs::ColorRGBA	last_collision_object_color_
arm_navigation_msgs::ArmNavigationErrorCodes	last_collision_set_error_code_
std::vector< ros::Time >	last_creation_time_query_
ros::Time	last_marker_start_time_
std_msgs::ColorRGBA	last_mesh_object_color_
interactive_markers::MenuHandler::EntryHandle	last_resize_handle_
ros::ServiceClient	left_ik_service_client_
ros::ServiceClient	left_interpolate_service_client_
ros::ServiceClient	list_controllers_client_
ros::ServiceClient	load_controllers_client_
boost::recursive_mutex	lock_scene_
std::string	logged_group_name_
std::string	logged_motion_plan_request_
trajectory_msgs::JointTrajectory	logged_trajectory_
trajectory_msgs::JointTrajectory	logged_trajectory_controller_error_
int	logged_trajectory_controller_error_code_
ros::Time	logged_trajectory_start_time_
ros::Duration	marker_dt_
unsigned int	max_collision_object_id_
unsigned int	max_trajectory_id_
std::map< std::string, MenuEntryMap >	menu_entry_maps_
MenuHandlerMap	menu_handler_map_
MonitorStatus	monitor_status_
move_arm_warehouse::MoveArmWarehouseLoggerReader *	move_arm_warehouse_logger_reader_
ros::NodeHandle	nh_
ros::ServiceClient	non_coll_left_ik_service_client_
ros::ServiceClient	non_coll_right_ik_service_client_
std::map< std::string, ros::ServiceClient * > *	non_collision_aware_ik_services_
PlanningSceneParameters	params_
ros::ServiceClient	pause_gazebo_client_
planning_models::KinematicState *	paused_collision_state_
ros::ServiceClient	planning_1_service_client_
ros::ServiceClient	planning_2_service_client_
std_msgs::ColorRGBA	point_color_
ros::Subscriber	r_arm_controller_state_subscriber_
ros::ServiceClient	right_ik_service_client_
ros::ServiceClient	right_interpolate_service_client_
planning_models::KinematicState *	robot_state_
std::map< std::string, double >	robot_state_joint_values_
std::vector < geometry_msgs::TransformStamped >	robot_transforms_
std::map< std::string, SelectableObject > *	selectable_objects_
std::string	selected_motion_plan_name_
std::string	selected_trajectory_name_
bool	send_collision_markers_
ros::ServiceClient	set_link_properties_client_
ros::ServiceClient	set_planning_scene_diff_client_
std::vector< StateRegistry >	states_
ros::ServiceClient	switch_controllers_client_
ros::Time	time_of_controller_done_callback_
ros::Time	time_of_last_moving_notification_
ros::ServiceClient	trajectory_filter_1_service_client_
ros::ServiceClient	trajectory_filter_2_service_client_
tf::TransformBroadcaster	transform_broadcaster_
tf::TransformListener	transform_listener_
ros::ServiceClient	unload_controllers_client_
ros::ServiceClient	unpause_gazebo_client_
bool	use_primary_filter_
ros::Publisher	vis_marker_array_publisher_
ros::Publisher	vis_marker_publisher_
bool	warehouse_data_loaded_once_

Detailed Description

Class for creating, editing, and saving planning scenes.

Class PlanningSceneEditor

Definition at line 1339 of file move_arm_utils.h.

Member Enumeration Documentation

enum planning_scene_utils::PlanningSceneEditor::GeneratedShape

These kinds of shapes can be created by the editor.

Enum GeneratedShape

Enumerator:

Box
Cylinder
Sphere

Definition at line 1346 of file move_arm_utils.h.

enum planning_scene_utils::PlanningSceneEditor::MonitorStatus [protected]

PlanningSceneEditor monitors robot state while "use_robot_data_" is true. When in idle mode, the monitor is not recording robot state into a trajectory. In Executing mode, the monitor records to a trajectory. In Done mode, the final trajectory is saved to the trajectory map.

Enum MonitorStatus

Enumerator:

idle
Executing
WaitingForStop
Done

Definition at line 1360 of file move_arm_utils.h.

Constructor & Destructor Documentation

PlanningSceneEditor::PlanningSceneEditor ( )

Definition at line 703 of file move_arm_utils.cpp.

PlanningSceneEditor::PlanningSceneEditor ( PlanningSceneParameters & params )

Memory initialization

Interactive markers

Publishers

Subscribers

Interactive menus

Connection with sim data

Connection with joint controller

Definition at line 720 of file move_arm_utils.cpp.

PlanningSceneEditor::~PlanningSceneEditor ( )

Definition at line 1127 of file move_arm_utils.cpp.

Member Function Documentation

void PlanningSceneEditor::armHasStoppedMoving ( )

Called when the robot actually stops moving, following execution of the trajectory.

Definition at line 4277 of file move_arm_utils.cpp.

void PlanningSceneEditor::attachCollisionObject	(	const std::string &	name,
		const std::string &	link_name,
		const std::vector< std::string > &	touch_links
	)		`[protected]`

Definition at line 3580 of file move_arm_utils.cpp.

void PlanningSceneEditor::attachedCollisionObjectInteractiveCallback ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback )

Definition at line 3435 of file move_arm_utils.cpp.

virtual void planning_scene_utils::PlanningSceneEditor::attachObjectCallback ( const std::string & name ) [protected, pure virtual]

Implemented in WarehouseViewer, and PlanningSceneEditorTest.

void PlanningSceneEditor::changeToAttached ( const std::string & name ) [protected]

Definition at line 3422 of file move_arm_utils.cpp.

void PlanningSceneEditor::collisionObjectMovementCallback ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback )

called when a collision object is moved in rviz.

Parameters:

feedback the change that occurred to the collision object.

Definition at line 3298 of file move_arm_utils.cpp.

void PlanningSceneEditor::collisionObjectSelectionCallback ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback )

called when a collision object is selected in rviz.

Parameters:

feedback the change that occurred to the collision object.

Definition at line 3256 of file move_arm_utils.cpp.

void PlanningSceneEditor::controllerDoneCallback	(	const actionlib::SimpleClientGoalState &	state,
		const control_msgs::FollowJointTrajectoryResultConstPtr &	result
	)

Called when the robot monitor detects that the robot has stopped following a trajectory.

Parameters:

state	the goal of the trajectory.
result	what the controller did while executing the trajectory.

Definition at line 4249 of file move_arm_utils.cpp.

std::string PlanningSceneEditor::createCollisionObject	(	const std::string &	name,
		geometry_msgs::Pose	pose,
		PlanningSceneEditor::GeneratedShape	shape,
		float	scaleX,
		float	scaleY,
		float	scaleZ,
		std_msgs::ColorRGBA	color
	)

creates an entirely new collision object and places it in the environment.

Parameters:

pose	the position and orientation of the object
shape	the type of object to create (Box, Cylinder, etc.)
scaleX	the size of the object in the x direction in meters.
scaleY	the size of the object in the y direction in meters.
scaleZ	the size of the object in the z direction in meters.
color	the color of the collision object.

Returns:: the unique id of the collision object

Definition at line 3522 of file move_arm_utils.cpp.

void PlanningSceneEditor::createIKController	(	MotionPlanRequestData &	data,
		PositionType	type,
		bool	rePose = `true`
	)

creates a 6DOF control over the end effector of either the start or goal position of the given request.

Parameters:

data	the motion plan request to create a 6DOF control over
type	either the start or goal position of the request
rePose,if	the interactive marker already exists, should it be re-posed?

Definition at line 3092 of file move_arm_utils.cpp.

void PlanningSceneEditor::createIkControllersFromMotionPlanRequest	(	MotionPlanRequestData &	data,
		bool	rePose = `true`
	)

creates both the start and goal 6DOF controls for the given request, but only if those are visible and editable.

Parameters:

data	the motion plan request to create 6DOF controls for.
rePose,if	the interactive markers already exist, should they be re-posed?

Definition at line 3077 of file move_arm_utils.cpp.

void PlanningSceneEditor::createJointMarkers	(	MotionPlanRequestData &	data,
		planning_scene_utils::PositionType	position
	)

creates 1DOF controls for each of the joints of the given request and its start and end positions.

Parameters:

data	the motion plan request to create joint controls for
position	either the start or goal position of the request.

Definition at line 3860 of file move_arm_utils.cpp.

std::string PlanningSceneEditor::createMeshObject	(	const std::string &	name,
		geometry_msgs::Pose	pose,
		const std::string &	filename,
		const tf::Vector3 &	scale,
		std_msgs::ColorRGBA	color
	)

Definition at line 3482 of file move_arm_utils.cpp.

void PlanningSceneEditor::createMotionPlanRequest	(	const planning_models::KinematicState &	start_state,
		const planning_models::KinematicState &	end_state,
		const std::string &	group_name,
		const std::string &	end_effector_name,
		const unsigned int &	planning_scene_name,
		const bool	fromRobotState,
		unsigned int &	motionPlan_id_Out
	)

Creates an entirely new motion plan request with the given parameters.

Parameters:

start_state	the kinematic state that the robot begins in.
end_state	the kinematic state to plan to.
group_name	the group that all plans will be performed for (joints outside the group are ignored)
end_effector_name	the link that IK will be solved for.
constrain	should the request constrain the pitch and roll of the end effector?
planning_scene_name	the id of the planning scene that this request occurs in.
motionPlan_id_Out	the id of the new motion plan request.
fromRobotState	should the request start from the robot's current state, ignoring start_state?

Definition at line 1677 of file move_arm_utils.cpp.

std::string PlanningSceneEditor::createNewPlanningScene ( )

creates an entirely new, empty planning scene.

Returns:: the newly generated id of the planning scene.

Definition at line 2049 of file move_arm_utils.cpp.

void PlanningSceneEditor::createSelectableMarkerFromCollisionObject	(	arm_navigation_msgs::CollisionObject &	object,
		std::string	name,
		std::string	description,
		std_msgs::ColorRGBA	color,
		bool	insert_selectable = `true`
	)		`[protected]`

Registers a collision object as a selectable marker.

Definition at line 2709 of file move_arm_utils.cpp.

void PlanningSceneEditor::deleteCollisionObject ( std::string & name )

Removes the collision object with the specified name from the world.

Parameters:

name	the unique id of the object.

Definition at line 3245 of file move_arm_utils.cpp.

void PlanningSceneEditor::deleteJointMarkers	(	MotionPlanRequestData &	data,
		PositionType	type
	)

erases all the interactive 1DOF markers on the joints of the given request.

Parameters:

data	the motion plan request to erase.
type	erase either the start or goal joint controls.

Definition at line 3833 of file move_arm_utils.cpp.

void PlanningSceneEditor::deleteKinematicStates ( )

All kinematic states in the editor are kept track of, and must be deleted with this function before the planning scene can be sent to the environment server. Otherwise, the editor will hang.

Definition at line 2245 of file move_arm_utils.cpp.

void PlanningSceneEditor::deleteMotionPlanRequest	(	const unsigned int &	id,
		std::vector< unsigned int > &	erased_trajectories
	)

erases the given motion plan request and all its associated trajectories.

Parameters:

id	the id of the motion plan request to delete

Definition at line 4421 of file move_arm_utils.cpp.

void PlanningSceneEditor::deleteTrajectory	(	unsigned int	mpr_id,
		unsigned int	traj_id
	)

erases the given trajectory from the trajectory map.

Parameters:

id	the id of the trajectory to delete.

Definition at line 4369 of file move_arm_utils.cpp.

void PlanningSceneEditor::executeTrajectory	(	const std::string &	mpr_name,
		const std::string &	traj_name
	)

Creates orientation constraints from a given robot state.

Parameters:

state	the state to find orientation constraints for
end_effector_link	the link whose pose should be constrained
goal_constraint	constraint filled by the function which maintains the pitch and roll of end effector.
path_constraint	constraint filled by the function which maintains the pitch and roll of end effector. if real robot data is being used, this can be used to send a trajectory to the robot for execution.
trajectory_id	the id of the trajectory to execute.

Definition at line 4475 of file move_arm_utils.cpp.

void PlanningSceneEditor::executeTrajectory ( TrajectoryData & data )

if real robot data is being used, this can be used to send a trajectory to the robot for execution.

Parameters:

data	the trajectory to execute.

Definition at line 4049 of file move_arm_utils.cpp.

virtual void planning_scene_utils::PlanningSceneEditor::filterCallback ( arm_navigation_msgs::ArmNavigationErrorCodes & errorCode ) [protected, pure virtual]

Virtual function called when the filter is invoked.

Parameters:

errorCode,the result of the filter call.

Implemented in WarehouseViewer, and PlanningSceneEditorTest.

bool PlanningSceneEditor::filterTrajectory	(	MotionPlanRequestData &	requestData,
		TrajectoryData &	trajectory,
		unsigned int &	filter_id
	)

Calls the trajectory filter service on the given trajectory.

Parameters:

requestData	the request associated with the trajectory.
trajectory	the trajectory to filter.
filter_id	the id of the filtered trajectory to be returned.

Returns:: true on filtering success, or false on failure.

Definition at line 1908 of file move_arm_utils.cpp.

std::string planning_scene_utils::PlanningSceneEditor::generateNewCollisionObjectId ( ) [inline]

generates a unique collision object id.

Returns:: the newly generated id.

Definition at line 2075 of file move_arm_utils.h.

unsigned int planning_scene_utils::PlanningSceneEditor::generateNewPlanningSceneId ( ) [inline]

generates a unique planning scene id.

Returns:: the newly generated id.

Definition at line 2088 of file move_arm_utils.h.

bool PlanningSceneEditor::getAllAssociatedMotionPlanRequests	(	const unsigned int	id,
		std::vector< unsigned int > &	ids,
		std::vector< std::string > &	stages,
		std::vector< arm_navigation_msgs::MotionPlanRequest > &	requests
	)

loads motion plan requests associated with the given timestamp from the warehouse.

Parameters:

time	the time stamp associated with the planning scene.
ids	vector of strings to be filled with the ids of motion plan requests associated with that time.
stages	vector of strings to be filled with the planning stages associated with each motion plan request.
requests	vector of MotionPlanRequests to be filled with the requests associated with the given time.

Returns:: true if query to warehouse was successful, false otherwise.

Definition at line 2236 of file move_arm_utils.cpp.

bool PlanningSceneEditor::getAllAssociatedPausedStates	(	const unsigned int	id,
		std::vector< ros::Time > &	paused_times
	)

loads all paused states from the warehouse associated with the given time stamp.

Parameters:

time	the time stamp of the planning scene
paused_times	vector of time stamps corresponding to each paused time.

Returns:: true if the query to warehouse was successful, false otherwise.

Definition at line 2611 of file move_arm_utils.cpp.

bool PlanningSceneEditor::getAllAssociatedTrajectorySources	(	const unsigned int	planning_id,
		const unsigned int	mpr_id,
		std::vector< unsigned int > &	trajectory_ids,
		std::vector< std::string > &	trajectory_sources
	)

loads all trajectory sources from the warehouse associated with the given time stamp.

Parameters:

time	the time stamp of the planning scene
trajectory_sources	a vector of strings to be filled with the trajectory sources (planner, filter, etc.)

Returns:: true if the query to the warehouse was successful, false otherwise.

Definition at line 2597 of file move_arm_utils.cpp.

bool PlanningSceneEditor::getAllPlanningSceneTimes	(	std::vector< ros::Time > &	planning_scene_times,
		std::vector< unsigned int > &	planning_scene_ids
	)

loads all planning scene times from the warehouse.

Parameters:

planning_scene_times a vector of time stamps corresponding to each planning scene

Returns:: true if the query to the warehouse was successful, false otherwise.

Definition at line 2207 of file move_arm_utils.cpp.

void PlanningSceneEditor::getAllRobotStampedTransforms	(	const planning_models::KinematicState &	state,
		std::vector< geometry_msgs::TransformStamped > &	trans_vector,
		const ros::Time &	stamp
	)

gets TF data for the given kinematic state.

Parameters:

state	the kinematic state to produce transforms for
trans_vector	the vector of TF transforms to be filled by the function.
stamp	the time stamp to apply to each transform.

Definition at line 4300 of file move_arm_utils.cpp.

planning_environment::CollisionModels* planning_scene_utils::PlanningSceneEditor::getCollisionModel ( ) [inline]

Definition at line 2111 of file move_arm_utils.h.

move_arm_warehouse::MoveArmWarehouseLoggerReader* planning_scene_utils::PlanningSceneEditor::getLoggerReader ( ) [inline]

Definition at line 2142 of file move_arm_utils.h.

void PlanningSceneEditor::getMotionPlanningMarkers ( visualization_msgs::MarkerArray & arr )

Fills the given array with mesh markers associated with all motion plan requests.

Parameters:

arr	the marker array to fill with mesh markers.

Get markers for the start

Get markers for the end.

Get collision markers for the start and end state.

TODO: Figure out why motion plans are occasionally NULL

Definition at line 1483 of file move_arm_utils.cpp.

bool planning_scene_utils::PlanningSceneEditor::getMotionPlanRequest	(	const ros::Time &	time,
		const std::string &	stage,
		arm_navigation_msgs::MotionPlanRequest &	mpr,
		std::string &	id,
		std::string &	planning_scene_id
	)

loads a specific motion plan request from the warehouse.

Parameters:

time	the time stamp of the planning scene to load from.
stage	the planning stage associated with the request
mpr	the MotionPlanRequest message to fill with data from the warehouse.
id	the id of the request to be generated.
planning_scene_id	the planning scene id associated with the given time.

Returns:: true if the query to the warehouse was successful, false otherwise

bool PlanningSceneEditor::getPausedState	(	const unsigned int	id,
		const ros::Time &	paused_time,
		head_monitor_msgs::HeadMonitorFeedback &	paused_state
	)

loads a specific paused state from the warehouse

Parameters:

time	the time stamp associated with the planning scene
paused_time	time when the paused state occurred.
paused_state	message to be filled by the warehouse.

Returns:: true if the query to teh warehouse was successful, false otherwise

Definition at line 2621 of file move_arm_utils.cpp.

bool PlanningSceneEditor::getPlanningSceneOutcomes	(	const unsigned int	id,
		std::vector< std::string > &	pipeline_stages,
		std::vector< arm_navigation_msgs::ArmNavigationErrorCodes > &	error_codes,
		std::map< std::string, arm_navigation_msgs::ArmNavigationErrorCodes > &	error_map
	)

loads all the error codes associated with a particular planning scene from the warehouse.

Parameters:

time	the time stamp of the planning scene
pipeline_stages	std:: of strings to be filled with all request stages (planner, filter, etc.)
error_codes	vector of arm navigation error codes to be filled by the warehouse.
error_map	associates each error code with a trajectory id. To be filled by the warehouse.

Returns:: true if the query to the warehouse was successful, false otherwise.

Definition at line 2029 of file move_arm_utils.cpp.

planning_models::KinematicState* planning_scene_utils::PlanningSceneEditor::getRobotState ( ) [inline]

Definition at line 2126 of file move_arm_utils.h.

void PlanningSceneEditor::getTrajectoryMarkers ( visualization_msgs::MarkerArray & arr )

Fills the given array with mesh markers associated with all trajectories.

Parameters:

arr	the marker array to fill with mesh markers.

Get collision markers associated with trajectory.

Definition at line 1342 of file move_arm_utils.cpp.

bool PlanningSceneEditor::hasTrajectory	(	const std::string &	mpr_name,
		const std::string &	traj_name
	)		const

Definition at line 4482 of file move_arm_utils.cpp.

void PlanningSceneEditor::IKControllerCallback ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback )

Called when a 6DOF interactive marker is altered in RVIZ.

Parameters:

feedback the change that occurred to the marker.

Definition at line 2851 of file move_arm_utils.cpp.

void PlanningSceneEditor::initMotionPlanRequestData	(	const unsigned int &	planning_scene_id,
		const std::vector< unsigned int > &	ids,
		const std::vector< std::string > &	stages,
		const std::vector< arm_navigation_msgs::MotionPlanRequest > &	requests
	)

fills the motion_plan_map with new motion plan requests initialized with warehouse data.

Parameters:

planning_scene_id	the id of the planning scene these requests occur in.
ids	a vector containing all the motion plan request ids
stages	a vector containing all the motion plan request stages.
requests	a vector containing all the motion plan request messages from the warehouse.

Definition at line 2478 of file move_arm_utils.cpp.

void PlanningSceneEditor::JointControllerCallback ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback )

Called when a 1DOF joint marker is altered in RVIZ.

Parameters:

feedback the change that occured to the marker.

Definition at line 2824 of file move_arm_utils.cpp.

void PlanningSceneEditor::jointStateCallback ( const sensor_msgs::JointStateConstPtr & joint_state )

Called when the robot monitor detects a change in the robot state.

Parameters:

joint_state the new state of the robot.

Definition at line 996 of file move_arm_utils.cpp.

void PlanningSceneEditor::jointTrajectoryControllerStateCallback ( const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr & joint_state )

Called when the arm controller has a state update.

Parameters:

joint control state of the arm controller.

Definition at line 1061 of file move_arm_utils.cpp.

void PlanningSceneEditor::loadAllWarehouseData ( )

gets all the motion plan requests, trajectories, and planning scenes from the warehouse.

Definition at line 2099 of file move_arm_utils.cpp.

bool PlanningSceneEditor::loadPlanningScene	(	const ros::Time &	time,
		const unsigned int	id
	)

loads a particular planning scene from the warehouse

Parameters:

time	the time stamp associated with the planning scene
id	the id of the planning scene to be filled by the function.

Returns:: true if the query to the warehouse was successful, false otherwise.

Definition at line 2217 of file move_arm_utils.cpp.

void planning_scene_utils::PlanningSceneEditor::lockScene ( ) [inline]

Definition at line 2101 of file move_arm_utils.h.

void PlanningSceneEditor::makeInteractive1DOFRotationMarker	(	tf::Transform	transform,
		tf::Vector3	axis,
		std::string	name,
		std::string	description,
		float	scale = `1.0f`,
		float	angle = `0.0f`
	)

creates an interactive marker for rotation joint control.

Parameters:

transform	the position and orientation of the marker.
axis	the axis of rotation
name	the id of the marker
desciption	the text to be displayed above the marker
scale	the size of the marker's radius, in meters.
angle	the initial angle of the marker about its axis.

Definition at line 3991 of file move_arm_utils.cpp.

void PlanningSceneEditor::makeInteractive1DOFTranslationMarker	(	tf::Transform	transform,
		tf::Vector3	axis,
		std::string	name,
		std::string	description,
		float	scale = `1.0f`,
		float	value = `0.0f`
	)

creates an interactive marker for prismatic joint control.

Parameters:

transform	the position and orientation of the marker.
axis	the axis of translation of the prismatic joint.
name	the id of the marker.
description	the text to display above the marker.
scale	the size of the marker in meters.
value	the initial translation of the prismatic joint along its axis.

Definition at line 3954 of file move_arm_utils.cpp.

void PlanningSceneEditor::makeSelectableAttachedObjectFromPlanningScene	(	const arm_navigation_msgs::PlanningScene &	scene,
		arm_navigation_msgs::AttachedCollisionObject &	att
	)

Definition at line 1138 of file move_arm_utils.cpp.

virtual void planning_scene_utils::PlanningSceneEditor::onPlanningSceneLoaded	(	int	scene,
		int	numScenes
	)		`[inline, virtual]`

pure virtual function that acts as a callback when a given scene was loaded. (Use case: things like loading bars)

Parameters:

scene	the index of the scene that was loaded
numScenes	the total number of scenes in the warehouse

Reimplemented in WarehouseViewer.

Definition at line 1773 of file move_arm_utils.h.

virtual void planning_scene_utils::PlanningSceneEditor::planCallback ( arm_navigation_msgs::ArmNavigationErrorCodes & errorCode ) [protected, pure virtual]

Virtual function called when the planner is invoked.

Parameters:

errorCode,the result of the plan.

Implemented in WarehouseViewer, and PlanningSceneEditorTest.

bool PlanningSceneEditor::planToKinematicState	(	const planning_models::KinematicState &	state,
		const std::string &	group_name,
		const std::string &	end_effector_name,
		unsigned int &	trajectoryid_Out,
		unsigned int &	planning_scene_id
	)

invokes the planner service to plan from the current robot state to the given kinematic state.

Parameters:

state	the state to plan to.
group_name	the group to invoke the request for.
end_effector_name	the link that IK was performed on.
constrain	should the planner constrain pitch and roll of the end effector?
trajectoryid_Out	the new planned trajectory id to be filled by the function.
planning_scene_name	the id of the planning scene that this plan occurs in.

Returns:: true if the planner was successful, and false otherwise

Definition at line 1754 of file move_arm_utils.cpp.

bool PlanningSceneEditor::planToRequest	(	MotionPlanRequestData &	data,
		unsigned int &	trajectoryid_Out
	)

invokes the planner to plan from the start position of the request to the goal position.

Parameters:

data	the motion plan request to plan for.
trajectoryid_Out	the new planned trajectory id to be filled by the function.

Returns:: true if the planner was successful, false otherwise.

Definition at line 1769 of file move_arm_utils.cpp.

bool PlanningSceneEditor::planToRequest	(	const std::string &	requestid,
		unsigned int &	trajectoryid_Out
	)

invokes the planner to plan from the start position of the request to the goal position.

Parameters:

requestid	the motion plan request to plan for.
trajectoryid_Out	the new planned trajectory id to be filled by the function.

Returns:: true if the planner was successful, false otherwise.

Definition at line 1764 of file move_arm_utils.cpp.

bool PlanningSceneEditor::playTrajectory	(	MotionPlanRequestData &	requestData,
		TrajectoryData &	data
	)

non-blocking call resetting the given trajectory and setting it to play and be visible.

Parameters:

requestData	the motion plan request associated with the trajectory
data	the trajectory to play.

Returns:: true if the trajectory can be played, false otherwise.

Definition at line 2632 of file move_arm_utils.cpp.

void PlanningSceneEditor::printTrajectoryPoint	(	const std::vector< std::string > &	joint_names,
		const std::vector< double > &	joint_values
	)

for debugging, prints the given trajectory point values.

Definition at line 1900 of file move_arm_utils.cpp.

void PlanningSceneEditor::randomlyPerturb	(	MotionPlanRequestData &	mpr,
		PositionType	type
	)

sets the motion plan request start or goal to a set of random joint values. Avoids collisions

Parameters:

mpr	the motion plan request to randomize.
type	either the start or goal of the motion plan request.

Definition at line 4158 of file move_arm_utils.cpp.

MenuHandler::EntryHandle PlanningSceneEditor::registerMenuEntry	(	std::string	menu,
		std::string	entryName,
		interactive_markers::MenuHandler::FeedbackCallback &	callback
	)

creates an interactive marker menu with the given name, associating it with a callback function.

Parameters:

menu	the menu handler to register the entry in.
entryName	the name of the menu entry.
callback	the function to call when this menu entry is pressed

Returns:: the menu handle of the registered entry.

Definition at line 4361 of file move_arm_utils.cpp.

MenuHandler::EntryHandle PlanningSceneEditor::registerSubMenuEntry	(	std::string	menu,
		std::string	name,
		std::string	subMenu,
		interactive_markers::MenuHandler::FeedbackCallback &	callback
	)

registers a new menu entry as a sub menu of an exitsting interactive marker menu entry

Parameters:

menu	the menu handler maintaining the menu.
name	the name of the entry to make a sub menu for.
subMenu	the name of the sub menu entry
callback	the function to call when this menu is clicked.

Returns:: the menu handle for the registered entry.

Definition at line 4352 of file move_arm_utils.cpp.

void PlanningSceneEditor::savePlanningScene	(	PlanningSceneData &	data,
		bool	copy = `false`
	)

Pushes the given planning scene to the warehouse with ros::WallTime::now() as its timestamp.

Parameters:

data	the planning scene to push to the warehouse.

Definition at line 2131 of file move_arm_utils.cpp.

virtual void planning_scene_utils::PlanningSceneEditor::selectedTrajectoryCurrentPointChanged ( unsigned int new_current_point ) [inline, protected, virtual]

Called when the selected trajectory of the selected motion plan updates its current point. Override to do something useful.

Parameters:

new_current_point the new current point in the selected trajectory.

Reimplemented in WarehouseViewer.

Definition at line 1446 of file move_arm_utils.h.

void PlanningSceneEditor::sendMarkers ( )

sends all stored mesh and sphere markers for collisions and links to rviz.

Definition at line 2012 of file move_arm_utils.cpp.

bool PlanningSceneEditor::sendPlanningScene ( PlanningSceneData & data )

sends a planning scene diff message to the environment server, updating the global planning scene.

Parameters:

data	the planning scene to send.

Returns:: true if sending the diff was successful, and false otherwise.

Definition at line 2291 of file move_arm_utils.cpp.

void PlanningSceneEditor::sendTransformsAndClock ( )

sends all TF transforms and a wall clock time to ROS.

Definition at line 4331 of file move_arm_utils.cpp.

void planning_scene_utils::PlanningSceneEditor::setCollisionModel	(	planning_environment::CollisionModels *	model,
		bool	shouldDelete = `false`
	)		`[inline]`

Definition at line 2116 of file move_arm_utils.h.

void PlanningSceneEditor::setCurrentPlanningScene	(	std::string	id,
		bool	loadRequests = `true`,
		bool	loadTrajectories = `true`
	)

loads the given planning scene from the warehouse.

Parameters:

id	the id of the planning scene to load.
loadRequests	should the motion plan requests be loaded as well?
loadTrajectories	should the trajectories be loaded as well?

Get rid of old interactive markers.

Make sure all old trajectories and MPRs are gone.

Load planning scene

Create collision object.

Load motion plan requests

Load trajectories

Definition at line 1170 of file move_arm_utils.cpp.

void PlanningSceneEditor::setIKControlsVisible	(	std::string	id,
		PositionType	type,
		bool	visible
	)

either shows or hides the 6DOF interactive markers associated with the given request.

Parameters:

id	the id of the motion plan request.
type	either the start or goal position of the request.
visible	should the 6DOF controller be shown or not?

Definition at line 4028 of file move_arm_utils.cpp.

void PlanningSceneEditor::setJointState	(	MotionPlanRequestData &	data,
		PositionType	position,
		std::string &	jointName,
		tf::Transform	value
	)

attempts to set the state of the given joint in the given motion plan request so that it matches the given transform.

Parameters:

data	the motion plan request to set joint states for.
position	either the start or goal position of the motion plan request.
jointName	the joint to set the state for.
value	the joint will attempt to match this position and orientation.

Definition at line 3751 of file move_arm_utils.cpp.

void planning_scene_utils::PlanningSceneEditor::setLoggerReader	(	move_arm_warehouse::MoveArmWarehouseLoggerReader *	loggerReader,
		bool	shouldDelete = `true`
	)		`[inline]`

Definition at line 2147 of file move_arm_utils.h.

void planning_scene_utils::PlanningSceneEditor::setRobotState	(	planning_models::KinematicState *	robot_state,
		bool	shouldDelete = `true`
	)		`[inline]`

Definition at line 2131 of file move_arm_utils.h.

bool PlanningSceneEditor::solveIKForEndEffectorPose	(	MotionPlanRequestData &	mpr,
		planning_scene_utils::PositionType	type,
		bool	coll_aware = `true`,
		double	change_redundancy = `0.0`
	)

invokes the inverse kinematics solver on the given motion plan requests' start or end state, setting the joint values of that state to the solution.

Parameters:

mpr	the motion plan request to solve IK for.
type	solve for either the start position or the goal position.
coll_aware	should the IK solution be constrained as collision-free?
constrain_pitch_and_roll	should the IK solution maintain the pitch and roll of the end effector?
change_redundancy	alters the redundant joint of the robot by the given amount.