Convenience class wrapping a trajectory message and its meta-data. More...
#include <move_arm_utils.h>
Public Types | |
enum | MarkerType { VISUAL, COLLISION, PADDED } |
Public Member Functions | |
void | advanceThroughTrajectory (int amount) |
Sets the current state of the trajectory to the current trajectory point + amount. Allows for negative values. Does not overshoot the trajectory's end or start. | |
void | advanceToNextClosestPoint (ros::Time time) |
Gets the closest point on the trajectory whereby the point.time_from_start <= time - playback_start_time_. | |
bool | areCollisionsVisible () const |
Returns true if the collision sphers are being published, and false otherwise. | |
unsigned int | getBadPoint () const |
Returns the trajectory point where an error occurred. | |
visualization_msgs::MarkerArray & | getCollisionMarkers () |
Returns an array of small red spheres associated with each collision point. | |
const std_msgs::ColorRGBA & | getColor () const |
Returns the color of the markers representing the current state being published in Rviz. | |
unsigned int | getCurrentPoint () const |
see setCurrentPoint | |
planning_models::KinematicState * | getCurrentState () |
Gets the current kinematic state displayed by the planning scene editor. This state is also checked for collisions. | |
const ros::Duration & | getDuration () const |
For planners, returns the time it took to plan the trajectory, for filters, the time it took to filter it, and for robot monitors, the time it took to execute the trajectory. | |
const std::string & | getGroupName () const |
Gets the planning group associated with the trajectory (usually right arm or left arm) | |
unsigned int | getId () const |
Returns the unique Id of the trajectory. | |
MarkerType | getMarkerType () const |
const unsigned int & | getMotionPlanRequestId () const |
See setMotionPlanRequestId. | |
const std::string & | getName () const |
unsigned int | getPlanningScendId () const |
RenderType | getRenderType () const |
Gets what mesh to display in RVIZ. | |
std::string | getSource () |
Returns the pipeline stage of the trajectory. | |
trajectory_msgs::JointTrajectory & | getTrajectory () |
Returns the underlying trajectory message. | |
trajectory_msgs::JointTrajectory & | getTrajectoryError () |
Returns the underlying trajectory. | |
TrajectoryRenderType | getTrajectoryRenderType () const |
Gets what trajectory rendering method to display in RVIZ. | |
size_t | getTrajectorySize () const |
Returns the number of discrete points in the trajectory. | |
bool | hasRefreshedColors () const |
Returns ture if the refresh counter has been exhausted, and false otherwise. | |
bool | hasStateChanged () const |
Returns true if the current state has been recently changed, and false otherwise. | |
void | hide () |
Shorthand for setVisible(false) | |
void | hideCollisions () |
Shorthand for setCollisionsVisible(false) | |
bool | isPlaying () const |
Returns true if the current state is automatically marching through trajectory points, and false ow. | |
bool | isVisible () const |
Returns true if the current state is being shown in rviz, and false otherwise. | |
void | play () |
Starts playback of trajectory. | |
void | refreshColors () |
Sets the refresh_counter and assocated booleans so that the planning scene editor will cease publishing markers for a while, allowing the color of the markers to change. | |
void | reset () |
Deletes the kinematic states associated with the trajectory. | |
void | setBadPoint (unsigned int point) |
Sets the point of the trajectory where an error occurred. | |
void | setCollisionsVisible (bool shown) |
see areCollisionVisible | |
void | setColor (const std_msgs::ColorRGBA &color) |
See getColor. | |
void | setCurrentPoint (unsigned int point) |
Sets the current joint trajectory point displayed in Rviz. | |
void | setCurrentState (planning_models::KinematicState *state) |
see getCurrentState | |
void | setDuration (const ros::Duration &duration) |
See getDuration. | |
void | setGroupname (const std::string &group_name) |
Sets the planning group name of the trajectory (usually right arm or left arm) | |
void | setGroupName (std::string name) |
Sets the plannign group name associated with the trajectory (usually right arm or left arm) | |
void | setHasRefreshedColors (bool refresh) |
See hasRefreshedColors. | |
void | setId (const unsigned int &id) |
Sets the unique Id of the trajectory. | |
void | setMarkerType (MarkerType mt) |
Sets whether padded trimeshes are to be shown. | |
void | setMotionPlanRequestId (const unsigned int &Id) |
Sets the unique Id corresponding to the motion plan request associated with this trajectory. | |
void | setPlanningSceneId (const unsigned int id) |
void | setRenderType (RenderType renderType) |
Sets what mesh to display in RVIZ. | |
void | setSource (const std::string &source) |
See getSource. | |
void | setStateChanged (bool changed) |
See hasStateChanged. | |
void | setTrajectory (const trajectory_msgs::JointTrajectory &trajectory) |
see getTrajectory | |
void | setTrajectoryError (const trajectory_msgs::JointTrajectory &trajectory_error) |
see getTrajectoryError | |
void | setTrajectoryRenderType (TrajectoryRenderType renderType) |
Sets what trajectory rendering method to display in RVIZ. | |
void | setVisible (bool visible) |
See isVisible. | |
bool | shouldRefreshColors () const |
Returns true if the trajectory's color has changed, and false otherwise. | |
void | show () |
Shorthand for setVisible(true) | |
void | showCollisions () |
Shorthand for setCollisionsVisible(true) | |
void | stop () |
Stops playback of trajectory. | |
TrajectoryData () | |
TrajectoryData (const unsigned int &id, const std::string &source, const std::string &group_name, const trajectory_msgs::JointTrajectory &trajectory) | |
TrajectoryData (const unsigned int &id, const std::string &source, const std::string &group_name, const trajectory_msgs::JointTrajectory &trajectory, const trajectory_msgs::JointTrajectory &trajectory_error) | |
void | updateCollisionMarkers (planning_environment::CollisionModels *cm_, MotionPlanRequestData &motionPlanRequest, ros::ServiceClient *distance_state_validity_service_client_) |
Checks the current state for collisions, and fills the collision marker array with red spheres for each collision point. | |
void | updateCurrentState () |
Sets the joint states of the current state to those specified by the joint trajectory. | |
Public Attributes | |
ros::Duration | refresh_timer_ |
This counter is exhausted when the trajectory's color has changed. | |
arm_navigation_msgs::ArmNavigationErrorCodes | trajectory_error_code_ |
Corresponds to the planning, filtering, or execution outcome of the trajectory. | |
Protected Attributes | |
visualization_msgs::MarkerArray | collision_markers_ |
bool | collisions_visible_ |
std_msgs::ColorRGBA | color_ |
planning_models::KinematicState * | current_state_ |
unsigned int | current_trajectory_point_ |
ros::Duration | duration_ |
std::string | group_name_ |
bool | has_refreshed_colors_ |
unsigned int | id_ |
bool | is_playing_ |
bool | is_visible_ |
MarkerType | marker_type_ |
unsigned int | motion_plan_request_Id_ |
std::string | name_ |
unsigned int | planning_scene_id_ |
ros::Time | playback_start_time_ |
RenderType | render_type_ |
bool | should_refresh_colors_ |
std::string | source_ |
bool | state_changed_ |
ros::Duration | time_to_stop_ |
trajectory_msgs::JointTrajectory | trajectory_ |
unsigned int | trajectory_bad_point_ |
trajectory_msgs::JointTrajectory | trajectory_error_ |
TrajectoryRenderType | trajectory_render_type_ |
Convenience class wrapping a trajectory message and its meta-data.
Class TrajectoryData
Definition at line 899 of file move_arm_utils.h.
Definition at line 903 of file move_arm_utils.h.
Definition at line 110 of file move_arm_utils.cpp.
planning_scene_utils::TrajectoryData::TrajectoryData | ( | const unsigned int & | id, |
const std::string & | source, | ||
const std::string & | group_name, | ||
const trajectory_msgs::JointTrajectory & | trajectory | ||
) |
planning_scene_utils::TrajectoryData::TrajectoryData | ( | const unsigned int & | id, |
const std::string & | source, | ||
const std::string & | group_name, | ||
const trajectory_msgs::JointTrajectory & | trajectory, | ||
const trajectory_msgs::JointTrajectory & | trajectory_error | ||
) |
void TrajectoryData::advanceThroughTrajectory | ( | int | amount | ) |
Sets the current state of the trajectory to the current trajectory point + amount. Allows for negative values. Does not overshoot the trajectory's end or start.
Definition at line 207 of file move_arm_utils.cpp.
void TrajectoryData::advanceToNextClosestPoint | ( | ros::Time | time | ) |
Gets the closest point on the trajectory whereby the point.time_from_start <= time - playback_start_time_.
Definition at line 166 of file move_arm_utils.cpp.
bool planning_scene_utils::TrajectoryData::areCollisionsVisible | ( | ) | const [inline] |
Returns true if the collision sphers are being published, and false otherwise.
Definition at line 979 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::getBadPoint | ( | ) | const [inline] |
Returns the trajectory point where an error occurred.
Definition at line 1128 of file move_arm_utils.h.
visualization_msgs::MarkerArray& planning_scene_utils::TrajectoryData::getCollisionMarkers | ( | ) | [inline] |
Returns an array of small red spheres associated with each collision point.
Definition at line 973 of file move_arm_utils.h.
const std_msgs::ColorRGBA& planning_scene_utils::TrajectoryData::getColor | ( | ) | const [inline] |
Returns the color of the markers representing the current state being published in Rviz.
Definition at line 1207 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::getCurrentPoint | ( | ) | const [inline] |
see setCurrentPoint
Definition at line 1122 of file move_arm_utils.h.
Gets the current kinematic state displayed by the planning scene editor. This state is also checked for collisions.
Definition at line 1082 of file move_arm_utils.h.
const ros::Duration& planning_scene_utils::TrajectoryData::getDuration | ( | ) | const [inline] |
For planners, returns the time it took to plan the trajectory, for filters, the time it took to filter it, and for robot monitors, the time it took to execute the trajectory.
Definition at line 1195 of file move_arm_utils.h.
const std::string& planning_scene_utils::TrajectoryData::getGroupName | ( | ) | const [inline] |
Gets the planning group associated with the trajectory (usually right arm or left arm)
Definition at line 1272 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::getId | ( | ) | const [inline] |
Returns the unique Id of the trajectory.
Definition at line 1255 of file move_arm_utils.h.
MarkerType planning_scene_utils::TrajectoryData::getMarkerType | ( | ) | const [inline] |
Definition at line 1170 of file move_arm_utils.h.
const unsigned int& planning_scene_utils::TrajectoryData::getMotionPlanRequestId | ( | ) | const [inline] |
See setMotionPlanRequestId.
Definition at line 1101 of file move_arm_utils.h.
const std::string& planning_scene_utils::TrajectoryData::getName | ( | void | ) | const [inline] |
Definition at line 1267 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::getPlanningScendId | ( | ) | const [inline] |
Definition at line 1110 of file move_arm_utils.h.
RenderType planning_scene_utils::TrajectoryData::getRenderType | ( | ) | const [inline] |
Gets what mesh to display in RVIZ.
Definition at line 1041 of file move_arm_utils.h.
std::string planning_scene_utils::TrajectoryData::getSource | ( | ) | [inline] |
Returns the pipeline stage of the trajectory.
Definition at line 1219 of file move_arm_utils.h.
trajectory_msgs::JointTrajectory& planning_scene_utils::TrajectoryData::getTrajectory | ( | ) | [inline] |
Returns the underlying trajectory message.
Definition at line 1225 of file move_arm_utils.h.
trajectory_msgs::JointTrajectory& planning_scene_utils::TrajectoryData::getTrajectoryError | ( | ) | [inline] |
Returns the underlying trajectory.
Definition at line 1243 of file move_arm_utils.h.
TrajectoryRenderType planning_scene_utils::TrajectoryData::getTrajectoryRenderType | ( | ) | const [inline] |
Gets what trajectory rendering method to display in RVIZ.
Definition at line 1053 of file move_arm_utils.h.
size_t planning_scene_utils::TrajectoryData::getTrajectorySize | ( | ) | const [inline] |
Returns the number of discrete points in the trajectory.
Definition at line 1003 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::hasRefreshedColors | ( | ) | const [inline] |
Returns ture if the refresh counter has been exhausted, and false otherwise.
Definition at line 1015 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::hasStateChanged | ( | ) | const [inline] |
Returns true if the current state has been recently changed, and false otherwise.
Definition at line 961 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::hide | ( | ) | [inline] |
Shorthand for setVisible(false)
Definition at line 1188 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::hideCollisions | ( | ) | [inline] |
Shorthand for setCollisionsVisible(false)
Definition at line 997 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::isPlaying | ( | ) | const [inline] |
Returns true if the current state is automatically marching through trajectory points, and false ow.
Definition at line 1140 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::isVisible | ( | ) | const [inline] |
Returns true if the current state is being shown in rviz, and false otherwise.
Definition at line 1159 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::play | ( | ) | [inline] |
Starts playback of trajectory.
Definition at line 1146 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::refreshColors | ( | ) | [inline] |
Sets the refresh_counter and assocated booleans so that the planning scene editor will cease publishing markers for a while, allowing the color of the markers to change.
Definition at line 1033 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::reset | ( | ) | [inline] |
Deletes the kinematic states associated with the trajectory.
Definition at line 1065 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setBadPoint | ( | unsigned int | point | ) | [inline] |
Sets the point of the trajectory where an error occurred.
Definition at line 1278 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setCollisionsVisible | ( | bool | shown | ) | [inline] |
see areCollisionVisible
Definition at line 985 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setColor | ( | const std_msgs::ColorRGBA & | color | ) | [inline] |
See getColor.
Definition at line 1213 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setCurrentPoint | ( | unsigned int | point | ) | [inline] |
Sets the current joint trajectory point displayed in Rviz.
Definition at line 1115 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setCurrentState | ( | planning_models::KinematicState * | state | ) | [inline] |
see getCurrentState
Definition at line 1088 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setDuration | ( | const ros::Duration & | duration | ) | [inline] |
See getDuration.
Definition at line 1201 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setGroupname | ( | const std::string & | group_name | ) | [inline] |
Sets the planning group name of the trajectory (usually right arm or left arm)
Definition at line 1134 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setGroupName | ( | std::string | name | ) | [inline] |
Sets the plannign group name associated with the trajectory (usually right arm or left arm)
Definition at line 1284 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setHasRefreshedColors | ( | bool | refresh | ) | [inline] |
See hasRefreshedColors.
Definition at line 1021 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setId | ( | const unsigned int & | id | ) | [inline] |
Sets the unique Id of the trajectory.
Definition at line 1261 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setMarkerType | ( | MarkerType | mt | ) | [inline] |
Sets whether padded trimeshes are to be shown.
Definition at line 1176 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setMotionPlanRequestId | ( | const unsigned int & | Id | ) | [inline] |
Sets the unique Id corresponding to the motion plan request associated with this trajectory.
Definition at line 1095 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setPlanningSceneId | ( | const unsigned int | id | ) | [inline] |
Definition at line 1106 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setRenderType | ( | RenderType | renderType | ) | [inline] |
Sets what mesh to display in RVIZ.
Definition at line 1047 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setSource | ( | const std::string & | source | ) | [inline] |
See getSource.
Definition at line 1231 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setStateChanged | ( | bool | changed | ) | [inline] |
See hasStateChanged.
Definition at line 967 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setTrajectory | ( | const trajectory_msgs::JointTrajectory & | trajectory | ) | [inline] |
see getTrajectory
Definition at line 1237 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setTrajectoryError | ( | const trajectory_msgs::JointTrajectory & | trajectory_error | ) | [inline] |
see getTrajectoryError
Definition at line 1249 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setTrajectoryRenderType | ( | TrajectoryRenderType | renderType | ) | [inline] |
Sets what trajectory rendering method to display in RVIZ.
Definition at line 1059 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::setVisible | ( | bool | visible | ) | [inline] |
See isVisible.
Definition at line 1165 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::shouldRefreshColors | ( | ) | const [inline] |
Returns true if the trajectory's color has changed, and false otherwise.
Definition at line 1009 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::show | ( | ) | [inline] |
Shorthand for setVisible(true)
Definition at line 1182 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::showCollisions | ( | ) | [inline] |
Shorthand for setCollisionsVisible(true)
Definition at line 991 of file move_arm_utils.h.
void planning_scene_utils::TrajectoryData::stop | ( | void | ) | [inline] |
Stops playback of trajectory.
Definition at line 1153 of file move_arm_utils.h.
void TrajectoryData::updateCollisionMarkers | ( | planning_environment::CollisionModels * | cm_, |
MotionPlanRequestData & | motionPlanRequest, | ||
ros::ServiceClient * | distance_state_validity_service_client_ | ||
) |
Checks the current state for collisions, and fills the collision marker array with red spheres for each collision point.
Definition at line 253 of file move_arm_utils.cpp.
void TrajectoryData::updateCurrentState | ( | ) |
Sets the joint states of the current state to those specified by the joint trajectory.
Definition at line 237 of file move_arm_utils.cpp.
visualization_msgs::MarkerArray planning_scene_utils::TrajectoryData::collision_markers_ [protected] |
Definition at line 931 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::collisions_visible_ [protected] |
Definition at line 922 of file move_arm_utils.h.
std_msgs::ColorRGBA planning_scene_utils::TrajectoryData::color_ [protected] |
Definition at line 924 of file move_arm_utils.h.
Definition at line 927 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::current_trajectory_point_ [protected] |
Definition at line 925 of file move_arm_utils.h.
Definition at line 928 of file move_arm_utils.h.
std::string planning_scene_utils::TrajectoryData::group_name_ [protected] |
Definition at line 913 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::has_refreshed_colors_ [protected] |
Definition at line 930 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::id_ [protected] |
Definition at line 911 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::is_playing_ [protected] |
Definition at line 920 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::is_visible_ [protected] |
Definition at line 918 of file move_arm_utils.h.
Definition at line 919 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::motion_plan_request_Id_ [protected] |
Definition at line 915 of file move_arm_utils.h.
std::string planning_scene_utils::TrajectoryData::name_ [protected] |
Definition at line 910 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::planning_scene_id_ [protected] |
Definition at line 914 of file move_arm_utils.h.
Definition at line 921 of file move_arm_utils.h.
This counter is exhausted when the trajectory's color has changed.
Definition at line 938 of file move_arm_utils.h.
Definition at line 932 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::should_refresh_colors_ [protected] |
Definition at line 929 of file move_arm_utils.h.
std::string planning_scene_utils::TrajectoryData::source_ [protected] |
Definition at line 912 of file move_arm_utils.h.
bool planning_scene_utils::TrajectoryData::state_changed_ [protected] |
Definition at line 923 of file move_arm_utils.h.
Definition at line 934 of file move_arm_utils.h.
trajectory_msgs::JointTrajectory planning_scene_utils::TrajectoryData::trajectory_ [protected] |
Definition at line 916 of file move_arm_utils.h.
unsigned int planning_scene_utils::TrajectoryData::trajectory_bad_point_ [protected] |
Definition at line 926 of file move_arm_utils.h.
trajectory_msgs::JointTrajectory planning_scene_utils::TrajectoryData::trajectory_error_ [protected] |
Definition at line 917 of file move_arm_utils.h.
arm_navigation_msgs::ArmNavigationErrorCodes planning_scene_utils::TrajectoryData::trajectory_error_code_ |
Corresponds to the planning, filtering, or execution outcome of the trajectory.
Definition at line 941 of file move_arm_utils.h.
Definition at line 933 of file move_arm_utils.h.