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
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00061 #include <unistd.h>
00062 #include <math.h>
00063
00064 #include "ros/ros.h"
00065
00066
00067 #include "tf/transform_listener.h"
00068
00069
00070
00071
00072 #include <geometry_msgs/PoseStamped.h>
00073 #include <geometry_msgs/Twist.h>
00074 #include <std_msgs/String.h>
00075
00076 #include <sensor_msgs/LaserScan.h>
00077
00078 #include "BaseDistance.h"
00079
00080 class SpeedFilter {
00081 private:
00082 BaseDistance dist_control_;
00083
00084 ros::NodeHandle n_;
00085 ros::Publisher pub_vel_;
00086 ros::Subscriber sub_vel_;
00087
00088 void input_vel(const geometry_msgs::Twist::ConstPtr& msg);
00089
00090 public:
00091 SpeedFilter();
00092 };
00093
00094 void SpeedFilter::input_vel(const geometry_msgs::Twist::ConstPtr& msg)
00095 {
00096 geometry_msgs::Twist t = *msg;
00097
00098 dist_control_.compute_distance_keeping(&(t.linear.x), &(t.linear.y), &(t.angular.z));
00099 pub_vel_.publish(t);
00100 }
00101
00102
00103 SpeedFilter::SpeedFilter() : n_("~")
00104 {
00105 sub_vel_ = n_.subscribe("/input_vel", 1, &SpeedFilter::input_vel, this);
00106 pub_vel_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
00107
00108
00109
00110 double front, rear, left, right, tolerance;
00111 n_.param("footprint/left", left, 0.309);
00112 n_.param("footprint/right", right, -0.309);
00113 n_.param("footprint/front", front, 0.43);
00114 n_.param("footprint/rear", rear, -0.43);
00115 n_.param("footprint/tolerance", tolerance, 0.0);
00116 dist_control_.setFootprint(front, rear, left, right, tolerance);
00117 }
00118
00119 int main(int argc, char *argv[])
00120 {
00121 ros::init(argc, argv, "speed_filter");
00122
00123 SpeedFilter sf;
00124
00125 ros::spin();
00126
00127 return 0;
00128 }