21 #include <ISM/common_type/RecognitionResult.hpp> 22 #include <asr_msgs/AsrAttributedPointCloud.h> 23 #include <asr_msgs/AsrAttributedPoint.h> 26 #include <geometry_msgs/Pose.h> 28 #include <dynamic_reconfigure/server.h> 29 #include <asr_ism_visualizations/ism_pose_prediction_visualizerConfig.h> 30 #include <asr_ism_visualizations/ism_valid_position_visualizerConfig.h> 45 reconfigure_server_ =
new dynamic_reconfigure::Server<asr_ism_visualizations::ism_pose_prediction_visualizerConfig>(nh);
49 reconfigure_server_vp_ =
new dynamic_reconfigure::Server<asr_ism_visualizations::ism_valid_position_visualizerConfig>(nh_vp);
65 void dynamicReconfCallback(asr_ism_visualizations::ism_pose_prediction_visualizerConfig &config, uint32_t level) {
96 void addVisualization(ISM::RecognitionResultPtr result, asr_msgs::AsrAttributedPointCloud attributed_point_cloud,
bool valid_position_viz);
158 dynamic_reconfigure::Server<asr_ism_visualizations::ism_pose_prediction_visualizerConfig>*
reconfigure_server_;
void setGlobalParams(double marker_lifetime, std::string base_frame)
double max_angle_in_overlay_
std::string object_axis_map_
VP stuff.
MarkerArray generatePredictionMarker(ISM::RecognitionResultPtr result)
generates prediction markers
std::map< std::string, boost::filesystem::path > mesh_resource_paths_
void updateObjectMarker()
updates marker of selected object
MarkerArray generateValidPositions(ISM::RecognitionResultPtr result)
generates markers for validPositionSpace visualization
~ISMPosePredictionVisualizerRVIZ()
dynamic_reconfigure::Server< asr_ism_visualizations::ism_valid_position_visualizerConfig > * reconfigure_server_vp_
ISMPosePredictionVisualizerRVIZ(const ros::Publisher &publisher, double bin_size, double max_projection_angle_deviation, const std::map< std::string, boost::filesystem::path > type_to_ressource_path_map, const ros::NodeHandle &nh, const ros::NodeHandle &nh_vp)
dynamic_reconfigure::Server< asr_ism_visualizations::ism_pose_prediction_visualizerConfig > * reconfigure_server_
std::map< ISM::Object, std::vector< ISM::PosePtr > > object_type_to_poses_map_
void nextObject()
increments object counter and uptates object marker
std::vector< ColorRGBA > dist_color_
#define ROS_DEBUG_STREAM(args)
visualization_msgs::MarkerArray MarkerArray
void clearAllMarkerOfTopic()
void calculateDistColor(ISM::ObjectPtr camera_object_ptr)
boost::shared_ptr< ISMPosePredictionVisualizerRVIZ > ISMPosePredictionVisualizerRVIZPtr
double pose_prediction_value_
double pose_prediction_alpha_
void addVisualization(ISM::RecognitionResultPtr result, asr_msgs::AsrAttributedPointCloud attributed_point_cloud, bool valid_position_viz)
void nextPose()
increments posecounter and uptates object marker
void clearAllMarkerOfTopic()
deletes all markers of topic and sets update to false
void dynamicReconfCallback(asr_ism_visualizations::ism_pose_prediction_visualizerConfig &config, uint32_t level)
double max_distance_in_overlay_
std::string base_frame_
PP stuff.
std::map< int, ISM::Object > id_to_object_name_map_
void dynamicReconfCallbackVP(asr_ism_visualizations::ism_valid_position_visualizerConfig &config, uint32_t level)