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 #if ROS_VERSION_MINIMUM(1,12,0) // kinetic
00029 #include <urdf_model/types.h>
00030 #include <urdf_world/types.h>
00031 #else
00032 namespace urdf {
00033 typedef boost::shared_ptr<ModelInterface> ModelInterfaceSharedPtr;
00034 typedef boost::shared_ptr<Link> LinkSharedPtr;
00035 typedef boost::shared_ptr<const Link> LinkConstSharedPtr;
00036 typedef boost::shared_ptr<Visual> VisualSharedPtr;
00037 typedef boost::shared_ptr<const Mesh> MeshConstSharedPtr;
00038 typedef boost::shared_ptr<const Cylinder> CylinderConstSharedPtr;
00039 typedef boost::shared_ptr<const Box> BoxConstSharedPtr;
00040 typedef boost::shared_ptr<const Sphere> SphereConstSharedPtr;
00041 }
00042 #endif
00043
00044
00045 using namespace urdf;
00046
00047 namespace im_utils{
00048 geometry_msgs::Transform Pose2Transform( const geometry_msgs::Pose pose_msg);
00049 geometry_msgs::Pose Transform2Pose( const geometry_msgs::Transform tf_msg);
00050 geometry_msgs::Pose UrdfPose2Pose( const urdf::Pose pose);
00051
00052 visualization_msgs::InteractiveMarkerControl makeCylinderMarkerControl(const geometry_msgs::PoseStamped &stamped, double length, double radius, const std_msgs::ColorRGBA &color, bool use_color);
00053 visualization_msgs::InteractiveMarkerControl makeBoxMarkerControl(const geometry_msgs::PoseStamped &stamped, Vector3 dim, const std_msgs::ColorRGBA &color, bool use_color);
00054 visualization_msgs::InteractiveMarkerControl makeSphereMarkerControl(const geometry_msgs::PoseStamped &stamped, double rad, const std_msgs::ColorRGBA &color, bool use_color);
00055 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);
00056 visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale);
00057 visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource,
00058 const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale, const std_msgs::ColorRGBA &color);
00059 void addMeshLinksControl(visualization_msgs::InteractiveMarker &im, LinkConstSharedPtr link, KDL::Frame previous_frame, bool use_color, std_msgs::ColorRGBA color, double scale);
00060 void addMeshLinksControl(visualization_msgs::InteractiveMarker &im, LinkConstSharedPtr link, KDL::Frame previous_frame, bool use_color, std_msgs::ColorRGBA color, double scale, bool root);
00061
00062 ModelInterfaceSharedPtr getModelInterface(std::string model_file);
00063 visualization_msgs::InteractiveMarker makeLinksMarker(LinkConstSharedPtr link, bool use_color, std_msgs::ColorRGBA color, geometry_msgs::PoseStamped marker_ps, geometry_msgs::Pose origin_pose);
00064
00065 visualization_msgs::InteractiveMarker makeFingerControlMarker(const char *name, geometry_msgs::PoseStamped ps);
00066 visualization_msgs::InteractiveMarker makeSandiaHandMarker(geometry_msgs::PoseStamped ps);
00067
00068 visualization_msgs::Marker makeSandiaFinger0Marker(std::string frame_id);
00069 visualization_msgs::Marker makeSandiaFinger1Marker(std::string frame_id);
00070 visualization_msgs::Marker makeSandiaFinger2Marker(std::string frame_id);
00071
00072 visualization_msgs::InteractiveMarker makeSandiaHandInteractiveMarker(geometry_msgs::PoseStamped ps, std::string hand, int finger, int link);
00073
00074 std::string getRosPathFromModelPath(std::string path);
00075 std::string getRosPathFromFullPath(std::string path);
00076 std::string getFullPathFromModelPath(std::string path);
00077 std::string getFilePathFromRosPath( std::string rospath);
00078
00079 geometry_msgs::Pose getPose( XmlRpc::XmlRpcValue val);
00080 double getXmlValue( XmlRpc::XmlRpcValue val );
00081 }
00082 #endif