Go to the documentation of this file.00001 #ifndef _MARKER_UTILS_H_
00002 #define _MARKER_UTILS_H_
00003
00004
00005 #include <ros/ros.h>
00006
00007 #include <tf/tf.h>
00008 #include <tf/transform_listener.h>
00009 #include <tf/transform_broadcaster.h>
00010
00011 #include <interactive_markers/interactive_marker_server.h>
00012
00013 #include <interactive_markers/menu_handler.h>
00014 #include <jsk_interactive_marker/SetPose.h>
00015 #include <jsk_interactive_marker/MarkerSetPose.h>
00016
00017 #include <math.h>
00018 #include <jsk_interactive_marker/MarkerMenu.h>
00019 #include <jsk_interactive_marker/MarkerPose.h>
00020
00021 #include <std_msgs/Int8.h>
00022 #include "urdf_parser/urdf_parser.h"
00023 #include <fstream>
00024
00025 #include <kdl/frames_io.hpp>
00026 #include <tf_conversions/tf_kdl.h>
00027
00028
00029 using namespace urdf;
00030
00031 namespace im_utils{
00032 geometry_msgs::Transform Pose2Transform( const geometry_msgs::Pose pose_msg);
00033 geometry_msgs::Pose Transform2Pose( const geometry_msgs::Transform tf_msg);
00034 geometry_msgs::Pose UrdfPose2Pose( const urdf::Pose pose);
00035
00036 visualization_msgs::InteractiveMarkerControl makeCylinderMarkerControl(const geometry_msgs::PoseStamped &stamped, double length, double radius, const std_msgs::ColorRGBA &color, bool use_color);
00037 visualization_msgs::InteractiveMarkerControl makeBoxMarkerControl(const geometry_msgs::PoseStamped &stamped, Vector3 dim, const std_msgs::ColorRGBA &color, bool use_color);
00038 visualization_msgs::InteractiveMarkerControl makeSphereMarkerControl(const geometry_msgs::PoseStamped &stamped, double rad, const std_msgs::ColorRGBA &color, bool use_color);
00039 visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale, const std_msgs::ColorRGBA &color, bool use_color);
00040 visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale);
00041 visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource,
00042 const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale, const std_msgs::ColorRGBA &color);
00043 void addMeshLinksControl(visualization_msgs::InteractiveMarker &im, boost::shared_ptr<const Link> link, KDL::Frame previous_frame, bool use_color, std_msgs::ColorRGBA color, double scale);
00044 void addMeshLinksControl(visualization_msgs::InteractiveMarker &im, boost::shared_ptr<const Link> link, KDL::Frame previous_frame, bool use_color, std_msgs::ColorRGBA color, double scale, bool root);
00045
00046 boost::shared_ptr<ModelInterface> getModelInterface(std::string model_file);
00047 visualization_msgs::InteractiveMarker makeLinksMarker(boost::shared_ptr<const Link> link, bool use_color, std_msgs::ColorRGBA color, geometry_msgs::PoseStamped marker_ps, geometry_msgs::Pose origin_pose);
00048
00049 visualization_msgs::InteractiveMarker makeFingerControlMarker(const char *name, geometry_msgs::PoseStamped ps);
00050 visualization_msgs::InteractiveMarker makeSandiaHandMarker(geometry_msgs::PoseStamped ps);
00051
00052 visualization_msgs::Marker makeSandiaFinger0Marker(std::string frame_id);
00053 visualization_msgs::Marker makeSandiaFinger1Marker(std::string frame_id);
00054 visualization_msgs::Marker makeSandiaFinger2Marker(std::string frame_id);
00055
00056 visualization_msgs::InteractiveMarker makeSandiaHandInteractiveMarker(geometry_msgs::PoseStamped ps, std::string hand, int finger, int link);
00057
00058 std::string getRosPathFromModelPath(std::string path);
00059 std::string getRosPathFromFullPath(std::string path);
00060 std::string getFullPathFromModelPath(std::string path);
00061 std::string getFilePathFromRosPath( std::string rospath);
00062
00063 geometry_msgs::Pose getPose( XmlRpc::XmlRpcValue val);
00064 double getXmlValue( XmlRpc::XmlRpcValue val );
00065 }
00066 #endif