Classes | Public Types | Public Member Functions | Protected Member Functions | Protected Attributes
WarehouseComponentsVisualizer Class Reference

List of all members.

Classes

struct  GroupCollection
struct  SelectableMarker
 Contains data for selectable markers. Stored in a large map. More...
struct  StateTrajectoryDisplay

Public Types

enum  IKControlType { StartPosition, EndPosition }
 Used to decide which kinematic state the user is controlling. The planner plans from start to end. More...
enum  InteractiveMarkerType { EndEffectorControl, JointControl, CollisionObject }
 May be for IK control, joint control, or collision objects. More...

Public Member Functions

void createCollisionPole (int num, Pose pose)
 Creates a collision pole with the given ID at location given by pose. Creates a selectable marker too.
void createSelectableJointMarkers (GroupCollection &gc)
 Given the group collection, creates a set of selectable markers for each joint in the group.
void deleteJointMarkers (GroupCollection &gc)
void deselectMarker (SelectableMarker &marker, tf::Transform transform)
 Removes the 6DOF control of the given marker and replaces it with a menu.
void determinePitchRollConstraintsGivenState (const WarehouseComponentsVisualizer::GroupCollection &gc, const planning_models::KinematicState &state, arm_navigation_msgs::OrientationConstraint &goal_constraint, arm_navigation_msgs::OrientationConstraint &path_constraint) const
bool doesGroupHaveGoodIKSolution (const string &group) const
bool doesGroupHaveGoodTrajectory (const string &group, const string &source) const
bool filterPlannerTrajectory (WarehouseComponentsVisualizer::GroupCollection &gc)
size_t getNumPlanningGroups ()
GroupCollectiongetPlanningGroup (unsigned int i)
bool isGroupName (const string &name)
 Returns true if the string of the given name corresponds to a kinematic chain in the robot.
bool isValidJointName (GroupCollection &gc, string name)
void logMotionPlanRequest (std::string &stage_name, arm_navigation_msgs::MotionPlanRequest &motion_plan_request)
 logs the given motion plan request to the warehouse, inside the most previously logged planning scene.
void logNewPlanningScene ()
 takes the current robot state and planning environment and logs it to the warehouse. Note that this creates a new planning scene entirely.
void logOutcome (std::string &pipeline_stage, ArmNavigationErrorCodes &error_codes)
 logs the given outcome to the warehouse, inside the most previously logged planning scene.
void logTrajectory (std::string &source, ros::Duration &production_time, trajectory_msgs::JointTrajectory &trajectory)
 logs the given trajectory to the warehouse, inside the most previously logged planning scene.
void makeInteractive1DOFRotationMarker (tf::Transform transform, tf::Vector3 axis, string name, string description, float scale=1.0f, float angle=0.0f)
void makeInteractive1DOFTranslationMarker (tf::Transform transform, tf::Vector3 axis, string name, string description, float scale=1.0f, float value=0.0f)
void makeInteractive6DOFMarker (bool fixed, tf::Transform transform, string name, string description, float scale=1.0f, bool pole=false)
 Creates an interactive 6DOF marker control.
InteractiveMarkerControl & makeInteractiveBoxControl (InteractiveMarker &msg, float alpha=1.0f)
 Creates a clickable, box shaped marker.
InteractiveMarkerControl & makeInteractiveCylinderControl (InteractiveMarker &msg, float alpha=1.0f)
 Creates a clickable, cylinder shaped marker.
Marker makeMarkerBox (InteractiveMarker &msg, float alpha=1.0f)
 Creates a box shaped marker.
Marker makeMarkerCylinder (InteractiveMarker &msg, float alpha=1.0f)
 Creates a cylinder shaped marker.
Marker makeMarkerSphere (InteractiveMarker &msg)
 Creates a sphere-shaped marker.
void makePoleContextMenu (tf::Transform transform, string name, string description, float scale=1.0f)
 Wrapper for makeSelectableMarker which assumes we are creating a collision pole.
void makeSelectableMarker (InteractiveMarkerType type, tf::Transform transform, string name, string description, float scale=1.0f, bool publish=true)
 Creates a marker that is initially a clickable menu. Upon selection it turns into a 6DOF control.
void makeTopLevelMenu ()
void moveEndEffectorMarkers (double vx, double vy, double vz, double vr, double vp, double vw, bool coll_aware=true)
void moveThroughTrajectory (WarehouseComponentsVisualizer::GroupCollection &gc, const string &source_name, int step)
int nextCollisionPole ()
 Generates a new collision pole ID.
bool planToEndEffectorState (WarehouseComponentsVisualizer::GroupCollection &gc)
bool playTrajectory (WarehouseComponentsVisualizer::GroupCollection &gc, const string &source_name, const trajectory_msgs::JointTrajectory &traj)
void processInteractiveFeedback (const InteractiveMarkerFeedbackConstPtr &feedback)
 Main function that handles interactive marker feedback of all kinds.
void publishJointStates (GroupCollection &gc)
 Sends the joint states of the given group collection to the robot state publisher.
void randomlyPerturb (WarehouseComponentsVisualizer::GroupCollection &gc)
void refreshEnvironment ()
 Sends all collision pole changes and changes to the robot state to the planning environment.
MenuHandler::EntryHandle registerMenuEntry (MenuHandler &handler, MenuEntryMap &map, string name)
 Creates a new menu entry for the given menu handler, putting its handle in the given map.
MenuHandler::EntryHandle registerSubMenuEntry (MenuHandler &handler, MenuEntryMap &map, string name, MenuHandler::EntryHandle subMenuHandle)
 Creates a new menu entry for the given menu handler, putting its handle in the given map.
void removeCollisionPole (int num)
 deletes the collision pole with the given ID.
void removeCollisionPoleByName (string id)
 deletes the collision pole with the given name.
void resetToLastGoodState (GroupCollection &gc)
bool selectableMarkerExists (string name)
 Returns true if a selectable marker of the given name exists in the marker map.
void selectMarker (SelectableMarker &marker, tf::Transform transform)
 Removes the menu marker given and replaces it with a 6DOF marker.
void selectPlanningGroup (unsigned int entry)
void sendMarkers ()
void sendPlanningScene ()
void sendTransforms ()
void setJointState (GroupCollection &gc, std::string &jointName, tf::Transform value)
 Sets the kinematic state of a joint in the given group collection.
void setNewEndEffectorPosition (GroupCollection &gc, tf::Transform &cur, bool coll_aware)
bool solveIKForEndEffectorPose (WarehouseComponentsVisualizer::GroupCollection &gc, bool coll_aware=true, bool constrain_pitch_and_roll=false, double change_redundancy=0.0)
 WarehouseComponentsVisualizer ()
 ~WarehouseComponentsVisualizer ()

Protected Member Functions

void deleteKinematicStates ()
tf::Transform toBulletTransform (geometry_msgs::Pose pose)
Pose toGeometryPose (tf::Transform transform)

Protected Attributes

CollisionModelscm_
bool collision_aware_
MenuHandler::EntryHandle collision_aware_handle_
map< string,
arm_navigation_msgs::CollisionObject
collision_poles_
 Map of collision pole names to messages sent to ROS.
bool constrain_rp_
MenuHandler::EntryHandle constrain_rp_handle_
string current_group_name_
MenuHandler::EntryHandle end_position_handle_
ros::ServiceClient get_planning_scene_client_
map< string, GroupCollectiongroup_map_
MenuHandler::EntryHandle ik_control_handle_
IKControlType ik_control_type_
boost::shared_ptr
< InteractiveMarkerServer
interactive_marker_server_
bool is_ik_control_active_
bool is_joint_control_active_
map< string, booljoint_clicked_map_
MenuHandler::EntryHandle joint_control_handle_
map< string, tf::Transformjoint_prev_transform_map_
ros::Publisher joint_state_publisher_
map< string, Poselast_ee_poses_
 Maps end effector link names to their previously recorded poses.
MotionPlanRequest last_motion_plan_request_
boost::recursive_mutex lock_
move_arm_warehouse::MoveArmWarehouseLoggerReaderlogger_
MenuMap menu_entry_maps_
 Maps MenuHandles to their names. Used to determine which menu entry is selected.
MenuHandlerMap menu_handler_map_
 Maps strings to menu handlers. This is used for convenience and extensibility.
ros::NodeHandle nh_
int num_collision_poles_
 Used to generate new IDs for collision poles. Not the actual number of poles.
ros::ServiceClient planner_service_client_
arm_navigation_msgs::PlanningScene planning_scene_
MenuHandler::FeedbackCallback process_function_ptr_
 Boost function pointer to the main interactive feedback function.
KinematicStaterobot_state_
map< string, SelectableMarkerselectable_markers_
 Maps selectable marker names to a struct containing their information.
MenuHandler::EntryHandle start_position_handle_
ros::ServiceClient trajectory_filter_service_client_
tf::TransformBroadcaster transform_broadcaster_
ros::Publisher vis_marker_array_publisher_
ros::Publisher vis_marker_publisher_

Detailed Description

Definition at line 92 of file warehouse_components_visualizer.cpp.


Member Enumeration Documentation

Used to decide which kinematic state the user is controlling. The planner plans from start to end.

Enumerator:
StartPosition 
EndPosition 

Definition at line 98 of file warehouse_components_visualizer.cpp.

May be for IK control, joint control, or collision objects.

Enumerator:
EndEffectorControl 
JointControl 
CollisionObject 

Definition at line 104 of file warehouse_components_visualizer.cpp.


Constructor & Destructor Documentation

Definition at line 252 of file warehouse_components_visualizer.cpp.

Definition at line 410 of file warehouse_components_visualizer.cpp.


Member Function Documentation

void WarehouseComponentsVisualizer::createCollisionPole ( int  num,
Pose  pose 
) [inline]

Creates a collision pole with the given ID at location given by pose. Creates a selectable marker too.

Parameters:
numthe ID of the pole. (must be unique)
posethe location and orientation of the pole.

Definition at line 488 of file warehouse_components_visualizer.cpp.

Given the group collection, creates a set of selectable markers for each joint in the group.

Parameters:
gcthe group collection to create joint markers for.

Definition at line 761 of file warehouse_components_visualizer.cpp.

Definition at line 749 of file warehouse_components_visualizer.cpp.

Definition at line 2354 of file warehouse_components_visualizer.cpp.

Removes the 6DOF control of the given marker and replaces it with a menu.

Parameters:
markera reference to the selectablemarker struct.
transformlocation of the marker when it is de-selected.

Definition at line 2100 of file warehouse_components_visualizer.cpp.

Definition at line 939 of file warehouse_components_visualizer.cpp.

bool WarehouseComponentsVisualizer::doesGroupHaveGoodIKSolution ( const string &  group) const [inline]

Definition at line 2286 of file warehouse_components_visualizer.cpp.

bool WarehouseComponentsVisualizer::doesGroupHaveGoodTrajectory ( const string &  group,
const string &  source 
) const [inline]

Definition at line 2295 of file warehouse_components_visualizer.cpp.

Definition at line 1243 of file warehouse_components_visualizer.cpp.

Returns the number of kinematic chains in the robot.

Returns:
size_t with number of chains of the robot.

Definition at line 2313 of file warehouse_components_visualizer.cpp.

Definition at line 2318 of file warehouse_components_visualizer.cpp.

bool WarehouseComponentsVisualizer::isGroupName ( const string &  name) [inline]

Returns true if the string of the given name corresponds to a kinematic chain in the robot.

Parameters:
namea string corresponding to a kinematic chain.
Returns:
true if the name is a kinematic chain, or false otherwise.

Definition at line 1935 of file warehouse_components_visualizer.cpp.

Definition at line 692 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::logMotionPlanRequest ( std::string &  stage_name,
arm_navigation_msgs::MotionPlanRequest motion_plan_request 
) [inline]

logs the given motion plan request to the warehouse, inside the most previously logged planning scene.

Definition at line 428 of file warehouse_components_visualizer.cpp.

takes the current robot state and planning environment and logs it to the warehouse. Note that this creates a new planning scene entirely.

Definition at line 446 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::logOutcome ( std::string &  pipeline_stage,
ArmNavigationErrorCodes error_codes 
) [inline]

logs the given outcome to the warehouse, inside the most previously logged planning scene.

Definition at line 420 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::logTrajectory ( std::string &  source,
ros::Duration production_time,
trajectory_msgs::JointTrajectory &  trajectory 
) [inline]

logs the given trajectory to the warehouse, inside the most previously logged planning scene.

Definition at line 436 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::makeInteractive1DOFRotationMarker ( tf::Transform  transform,
tf::Vector3  axis,
string  name,
string  description,
float  scale = 1.0f,
float  angle = 0.0f 
) [inline]

Definition at line 2162 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::makeInteractive1DOFTranslationMarker ( tf::Transform  transform,
tf::Vector3  axis,
string  name,
string  description,
float  scale = 1.0f,
float  value = 0.0f 
) [inline]

Definition at line 2125 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::makeInteractive6DOFMarker ( bool  fixed,
tf::Transform  transform,
string  name,
string  description,
float  scale = 1.0f,
bool  pole = false 
) [inline]

Creates an interactive 6DOF marker control.

Parameters:
fixedif true, the axes remain fixed to the frame of the marker. Otherwise they rotate with input.
transformthe location and rotation of the 6DOF marker.
nameused internally to represent the marker. Must be unique.
descriptiondrawn above the marker as a label.
scaleuniformly sets the size in meters of the marker.
poleif true, the marker is a large green cylinder. Otherwise it is a small white cube.

Definition at line 2208 of file warehouse_components_visualizer.cpp.

InteractiveMarkerControl& WarehouseComponentsVisualizer::makeInteractiveBoxControl ( InteractiveMarker &  msg,
float  alpha = 1.0f 
) [inline]

Creates a clickable, box shaped marker.

Parameters:
msgthe interactive marker to associate this box with.
alphathe transparency of the marker.
Returns:
the control (which is a clickable box)

Definition at line 1582 of file warehouse_components_visualizer.cpp.

InteractiveMarkerControl& WarehouseComponentsVisualizer::makeInteractiveCylinderControl ( InteractiveMarker &  msg,
float  alpha = 1.0f 
) [inline]

Creates a clickable, cylinder shaped marker.

Parameters:
msgthe interactive marker to associate this cylinder with.
alphathe transparency of the marker.
Returns:
the control (which is a clickable cylinder)

Definition at line 1597 of file warehouse_components_visualizer.cpp.

Marker WarehouseComponentsVisualizer::makeMarkerBox ( InteractiveMarker &  msg,
float  alpha = 1.0f 
) [inline]

Creates a box shaped marker.

Parameters:
msgthe interactive marker to associate this box with.
alphathe transparency of the marker.
Returns:
the marker (which is a box).

Definition at line 1516 of file warehouse_components_visualizer.cpp.

Marker WarehouseComponentsVisualizer::makeMarkerCylinder ( InteractiveMarker &  msg,
float  alpha = 1.0f 
) [inline]

Creates a cylinder shaped marker.

Parameters:
msgthe interactive marker to associate this cylinder with.
alphathe transparency of the marker.
Returns:
the marker (which is a cylinder).

Definition at line 1538 of file warehouse_components_visualizer.cpp.

Marker WarehouseComponentsVisualizer::makeMarkerSphere ( InteractiveMarker &  msg) [inline]

Creates a sphere-shaped marker.

Parameters:
msgthe interactive marker to associate this sphere with.
Returns:
the marker (which is a sphere)

Definition at line 1559 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::makePoleContextMenu ( tf::Transform  transform,
string  name,
string  description,
float  scale = 1.0f 
) [inline]

Wrapper for makeSelectableMarker which assumes we are creating a collision pole.

Definition at line 1975 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::makeSelectableMarker ( InteractiveMarkerType  type,
tf::Transform  transform,
string  name,
string  description,
float  scale = 1.0f,
bool  publish = true 
) [inline]

Creates a marker that is initially a clickable menu. Upon selection it turns into a 6DOF control.

Parameters:
typethe type (collision object, joint, IK control, etc.) of the selectable marker.
transformlocation and orientation of the marker.
nameinternal, unique representation of the marker (this is for the 6DOF control)
descriptiondisplayed above the menu marker and the 6DOF marker.
scaleuniformly sizes the marker and its controls
publishif true, the marker server will publish the marker. Otherwise, it will not.

Definition at line 1990 of file warehouse_components_visualizer.cpp.

Definition at line 1940 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::moveEndEffectorMarkers ( double  vx,
double  vy,
double  vz,
double  vr,
double  vp,
double  vw,
bool  coll_aware = true 
) [inline]

Definition at line 704 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::moveThroughTrajectory ( WarehouseComponentsVisualizer::GroupCollection gc,
const string &  source_name,
int  step 
) [inline]

Definition at line 1316 of file warehouse_components_visualizer.cpp.

Generates a new collision pole ID.

Returns:
an int, which is unique.

Definition at line 456 of file warehouse_components_visualizer.cpp.

Definition at line 1121 of file warehouse_components_visualizer.cpp.

bool WarehouseComponentsVisualizer::playTrajectory ( WarehouseComponentsVisualizer::GroupCollection gc,
const string &  source_name,
const trajectory_msgs::JointTrajectory &  traj 
) [inline]

Definition at line 1279 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::processInteractiveFeedback ( const InteractiveMarkerFeedbackConstPtr &  feedback) [inline]

Main function that handles interactive marker feedback of all kinds.

Parameters:
feedbackthe feedback message received by ROS. TODO: Replace this with several feedback functions.

Definition at line 1624 of file warehouse_components_visualizer.cpp.

Sends the joint states of the given group collection to the robot state publisher.

Parameters:
gcthe group collection to publish the states for.

Definition at line 1091 of file warehouse_components_visualizer.cpp.

Definition at line 1194 of file warehouse_components_visualizer.cpp.

Sends all collision pole changes and changes to the robot state to the planning environment.

Definition at line 1609 of file warehouse_components_visualizer.cpp.

Creates a new menu entry for the given menu handler, putting its handle in the given map.

Parameters:
handlerthe menu handler to associate this entry with.
mapthe menu entry map to associate the name with a handle.
thename which appears in the menu.
Returns:
the handle that was registered

Definition at line 1373 of file warehouse_components_visualizer.cpp.

Creates a new menu entry for the given menu handler, putting its handle in the given map.

Parameters:
handlerthe menu handler to associate this entry with.
mapthe menu entry map to associate the name with a handle.
thename which appears in the menu.
subMenuHandlea menu handle to an existing menu entry in the menu handler given.
Returns:
the handle that was registered

Definition at line 1358 of file warehouse_components_visualizer.cpp.

deletes the collision pole with the given ID.

Parameters:
numthe ID of the pole.

Definition at line 465 of file warehouse_components_visualizer.cpp.

deletes the collision pole with the given name.

Parameters:
idthe name of the pole.

Definition at line 476 of file warehouse_components_visualizer.cpp.

Definition at line 911 of file warehouse_components_visualizer.cpp.

Returns true if a selectable marker of the given name exists in the marker map.

Parameters:
namethe unique identifier of the marker.
Returns:
true if the marker exists, or false otherwise.

Definition at line 2055 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::selectMarker ( SelectableMarker marker,
tf::Transform  transform 
) [inline]

Removes the menu marker given and replaces it with a 6DOF marker.

Parameters:
markera reference to the selectablemarker struct.
transformlocation to select the marker.

Definition at line 2065 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::selectPlanningGroup ( unsigned int  entry) [inline]

Definition at line 620 of file warehouse_components_visualizer.cpp.

Definition at line 1380 of file warehouse_components_visualizer.cpp.

Definition at line 513 of file warehouse_components_visualizer.cpp.

Definition at line 1489 of file warehouse_components_visualizer.cpp.

void WarehouseComponentsVisualizer::setJointState ( GroupCollection gc,
std::string &  jointName,
tf::Transform  value 
) [inline]

Sets the kinematic state of a joint in the given group collection.

Parameters:
gcthe group collection that the joint is in.
value,atransform that the joint will attempt to match.

Definition at line 850 of file warehouse_components_visualizer.cpp.

Definition at line 921 of file warehouse_components_visualizer.cpp.

bool WarehouseComponentsVisualizer::solveIKForEndEffectorPose ( WarehouseComponentsVisualizer::GroupCollection gc,
bool  coll_aware = true,
bool  constrain_pitch_and_roll = false,
double  change_redundancy = 0.0 
) [inline]

Definition at line 965 of file warehouse_components_visualizer.cpp.

Definition at line 2347 of file warehouse_components_visualizer.cpp.

Definition at line 2334 of file warehouse_components_visualizer.cpp.


Member Data Documentation

Definition at line 2367 of file warehouse_components_visualizer.cpp.

Definition at line 2395 of file warehouse_components_visualizer.cpp.

Definition at line 2392 of file warehouse_components_visualizer.cpp.

Map of collision pole names to messages sent to ROS.

Definition at line 2377 of file warehouse_components_visualizer.cpp.

Definition at line 2394 of file warehouse_components_visualizer.cpp.

Definition at line 2393 of file warehouse_components_visualizer.cpp.

Definition at line 2412 of file warehouse_components_visualizer.cpp.

Definition at line 2389 of file warehouse_components_visualizer.cpp.

Definition at line 2408 of file warehouse_components_visualizer.cpp.

Definition at line 2374 of file warehouse_components_visualizer.cpp.

Definition at line 2390 of file warehouse_components_visualizer.cpp.

Definition at line 2362 of file warehouse_components_visualizer.cpp.

Definition at line 2365 of file warehouse_components_visualizer.cpp.

Definition at line 2416 of file warehouse_components_visualizer.cpp.

Definition at line 2417 of file warehouse_components_visualizer.cpp.

Definition at line 2419 of file warehouse_components_visualizer.cpp.

Definition at line 2391 of file warehouse_components_visualizer.cpp.

Definition at line 2420 of file warehouse_components_visualizer.cpp.

Definition at line 2405 of file warehouse_components_visualizer.cpp.

Maps end effector link names to their previously recorded poses.

Definition at line 2380 of file warehouse_components_visualizer.cpp.

Definition at line 2402 of file warehouse_components_visualizer.cpp.

boost::recursive_mutex WarehouseComponentsVisualizer::lock_ [protected]

Definition at line 2364 of file warehouse_components_visualizer.cpp.

Definition at line 2422 of file warehouse_components_visualizer.cpp.

Maps MenuHandles to their names. Used to determine which menu entry is selected.

Definition at line 2400 of file warehouse_components_visualizer.cpp.

Maps strings to menu handlers. This is used for convenience and extensibility.

Definition at line 2397 of file warehouse_components_visualizer.cpp.

Definition at line 2404 of file warehouse_components_visualizer.cpp.

Used to generate new IDs for collision poles. Not the actual number of poles.

Definition at line 2370 of file warehouse_components_visualizer.cpp.

Definition at line 2409 of file warehouse_components_visualizer.cpp.

Definition at line 2423 of file warehouse_components_visualizer.cpp.

Boost function pointer to the main interactive feedback function.

Definition at line 2386 of file warehouse_components_visualizer.cpp.

Definition at line 2372 of file warehouse_components_visualizer.cpp.

Maps selectable marker names to a struct containing their information.

Definition at line 2383 of file warehouse_components_visualizer.cpp.

Definition at line 2388 of file warehouse_components_visualizer.cpp.

Definition at line 2410 of file warehouse_components_visualizer.cpp.

Definition at line 2414 of file warehouse_components_visualizer.cpp.

Definition at line 2406 of file warehouse_components_visualizer.cpp.

Definition at line 2407 of file warehouse_components_visualizer.cpp.


The documentation for this class was generated from the following file:


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Thu Dec 12 2013 11:09:12