Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <string>
00019
00020 #include <ros/ros.h>
00021 #include <tf/tf.h>
00022 #include <tf/transform_listener.h>
00023 #include <std_msgs/ColorRGBA.h>
00024 #include <visualization_msgs/MarkerArray.h>
00025 #include <cob_twist_controller/TwistControllerConfig.h>
00026
00027 class DebugTrajectoryMarker
00028 {
00029 ros::NodeHandle nh_;
00030 ros::Publisher marker_pub_;
00031 tf::TransformListener tf_listener_;
00032
00033 ros::Timer marker_timer_;
00034
00035 std::string root_frame_;
00036 std::string base_link_;
00037 std::string chain_tip_link_;
00038 std::string target_frame_;
00039
00040 visualization_msgs::Marker tip_marker_;
00041 visualization_msgs::Marker target_marker_;
00042 visualization_msgs::Marker base_marker_;
00043
00044 public:
00045 int init()
00046 {
00047 if (!nh_.getParam("root_frame", this->root_frame_))
00048 {
00049 ROS_ERROR("Failed to get parameter \"root_frame\".");
00050 return -1;
00051 }
00052
00053 this->base_link_ = "base_link";
00054
00055 if (!nh_.getParam("chain_tip_link", this->chain_tip_link_))
00056 {
00057 ROS_ERROR("Failed to get parameter \"chain_tip_link\".");
00058 return -2;
00059 }
00060
00061 if (!nh_.getParam("frame_tracker/target_frame", this->target_frame_))
00062 {
00063 ROS_ERROR_STREAM("Please provide a 'frame_tracker/target_frame' parameter in this node's namespace.");
00064 return -3;
00065 }
00066
00067 marker_pub_ = this->nh_.advertise<visualization_msgs::MarkerArray>("trajectory_marker", 1, true);
00068 marker_timer_ = this->nh_.createTimer(ros::Duration(0.1), &DebugTrajectoryMarker::publishMarker, this);
00069
00070 while (marker_pub_.getNumSubscribers() < 1)
00071 {
00072 if (ros::isShuttingDown())
00073 {
00074 return -4;
00075 }
00076 ROS_WARN_STREAM_ONCE("Please create a subscriber to '" + this->nh_.getNamespace() + "/trajectory_marker' topic (Type: visualization_msgs/MarkerArray)");
00077 ros::Duration(1.0).sleep();
00078 }
00079
00080 std_msgs::ColorRGBA green;
00081 green.g = green.a = 1.0;
00082 tip_marker_ = getMarker(green);
00083 tip_marker_.ns = "tip_marker";
00084
00085 std_msgs::ColorRGBA red;
00086 red.r = red.a = 1.0;
00087 target_marker_ = getMarker(red);
00088 target_marker_.ns = "target_marker";
00089
00090 std_msgs::ColorRGBA blue;
00091 blue.b = blue.a = 1.0;
00092 base_marker_ = getMarker(blue);
00093 base_marker_.ns = "base_marker";
00094
00095 marker_timer_.start();
00096
00097 return 0;
00098 }
00099
00100 void publishMarker(const ros::TimerEvent& event)
00101 {
00102 visualization_msgs::MarkerArray marker_array;
00103 geometry_msgs::Point point;
00104
00105 if (tf_listener_.frameExists(this->chain_tip_link_))
00106 {
00107 if (tip_marker_.points.size() > 10000)
00108 {
00109 tip_marker_.points.clear();
00110 }
00111 point = getPoint(this->root_frame_, this->chain_tip_link_);
00112 tip_marker_.points.push_back(point);
00113 marker_array.markers.push_back(tip_marker_);
00114 }
00115
00116 if (tf_listener_.frameExists(this->target_frame_))
00117 {
00118 if (target_marker_.points.size() > 10000)
00119 {
00120 target_marker_.points.clear();
00121 }
00122 point = getPoint(this->root_frame_, this->target_frame_);
00123 target_marker_.points.push_back(point);
00124 marker_array.markers.push_back(target_marker_);
00125 }
00126
00127 if (tf_listener_.frameExists(this->base_link_))
00128 {
00129 if (nh_.param("twist_controller/kinematic_extension", 0) == cob_twist_controller::TwistController_BASE_ACTIVE)
00130 {
00131 if (base_marker_.points.size() > 10000)
00132 {
00133 base_marker_.points.clear();
00134 }
00135 point = getPoint(this->root_frame_, this->base_link_);
00136 base_marker_.points.push_back(point);
00137 marker_array.markers.push_back(base_marker_);
00138 }
00139 }
00140
00141 if (!marker_array.markers.empty())
00142 {
00143 this->marker_pub_.publish(marker_array);
00144 }
00145 }
00146
00147 visualization_msgs::Marker getMarker(std_msgs::ColorRGBA color)
00148 {
00149 visualization_msgs::Marker box_marker;
00150 box_marker.type = visualization_msgs::Marker::LINE_STRIP;
00151 box_marker.lifetime = ros::Duration();
00152 box_marker.action = visualization_msgs::Marker::ADD;
00153 box_marker.id = 42;
00154 box_marker.header.stamp = ros::Time::now();
00155 box_marker.header.frame_id = this->root_frame_;
00156
00157 box_marker.scale.x = 0.01;
00158 box_marker.color = color;
00159 box_marker.pose.orientation.w = 1.0;
00160
00161 return box_marker;
00162 }
00163
00164 geometry_msgs::Point getPoint(std::string from, std::string to)
00165 {
00166 geometry_msgs::Point point;
00167 tf::StampedTransform transform;
00168 try
00169 {
00170 ros::Time now = ros::Time::now();
00171 tf_listener_.waitForTransform(from, to, now, ros::Duration(0.5));
00172 tf_listener_.lookupTransform(from, to, now, transform);
00173
00174 point.x = transform.getOrigin().x();
00175 point.y = transform.getOrigin().y();
00176 point.z = transform.getOrigin().z();
00177 }
00178 catch (tf::TransformException& ex)
00179 {
00180 ROS_ERROR("%s", ex.what());
00181 }
00182
00183 return point;
00184 }
00185 };
00186
00187
00188 int main(int argc, char** argv)
00189 {
00190 ros::init(argc, argv, "debug_trajectory_marker_node");
00191
00192 DebugTrajectoryMarker dtm;
00193 if (dtm.init() != 0)
00194 {
00195 ROS_ERROR("Failed to initialize DebugTrajectoryMarker.");
00196 return -1;
00197 }
00198
00199 ros::spin();
00200 }