Merge pull request #140 from CPFL/develop-twist-filter

Filter lateral acceleration
autowarefoundation · Nov 19, 2015 · 56cf45f · 56cf45f
2 parents 10758a2 + 05affa3
commit 56cf45f
Show file tree

Hide file tree

Showing 7 changed files with 53 additions and 22 deletions.
diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt b/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt
@@ -67,5 +67,7 @@ target_link_libraries(odom_gen libwaypoint_follower ${catkin_LIBRARIES})
 
 add_executable(twist_filter nodes/twist_filter/twist_filter.cpp)
 target_link_libraries(twist_filter ${catkin_LIBRARIES})
+add_dependencies(twist_filter
+runtime_manager_generate_messages_cpp)
 
 
diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/launch/twist_filter.launch b/ros/src/computing/planning/motion/packages/waypoint_follower/launch/twist_filter.launch
@@ -0,0 +1,8 @@
+<!-- -->
+<launch>
+
+	<!-- rosrun waypoint_follower twist_filter -->
+	<node pkg="waypoint_follower" type="twist_filter" name="twist_filter" output="screen">
+	</node>
+
+</launch>
diff --git a/.../computing/planning/motion/packages/waypoint_follower/nodes/twist_filter/twist_filter.cpp b/.../computing/planning/motion/packages/waypoint_follower/nodes/twist_filter/twist_filter.cpp
@@ -32,38 +32,45 @@
 #include <geometry_msgs/TwistStamped.h>
 
 #include <iostream>
-#include <chrono>
-
-#define LOOP_RATE 100
-#define FILTER_SIZE 3
 
+#include "runtime_manager/ConfigTwistFilter.h"
 
 //Publisher
 static ros::Publisher g_twist_pub;
+static double g_lateral_accel_limit = 0.8;
 
+static void configCallback(const runtime_manager::ConfigTwistFilterConstPtr &config)
+{
+  g_lateral_accel_limit = config->lateral_accel_limit;
+  ROS_INFO("g_lateral_accel_limit = %lf",g_lateral_accel_limit);
+}
 
 void TwistCmdCallback(const geometry_msgs::TwistStampedConstPtr &msg)
 {
   geometry_msgs::TwistStamped twist;
+  twist.twist.linear.x = msg->twist.linear.x;
 
-  static std::list<geometry_msgs::Twist> twist_list;
-  twist_list.push_back(msg->twist);
+  double accel = msg->twist.linear.x * msg->twist.angular.z;
+  ROS_INFO("lateral accel = %lf",accel);
 
-  if(twist_list.size() != FILTER_SIZE)
-    return;
-  else{
-    for(std::list<geometry_msgs::Twist>::const_iterator it = twist_list.begin(); it != twist_list.end();it++){
-      twist.twist.linear.x += it->linear.x;
-      twist.twist.angular.z += it->angular.z;
-    }
-    twist.twist.linear.x /= FILTER_SIZE;
-    twist.twist.angular.z /= FILTER_SIZE;
 
-    twist.header = msg->header;
-    g_twist_pub.publish(twist);
 
-    twist_list.pop_front();
+  if(fabs(accel) > g_lateral_accel_limit)
+  {
+    ROS_INFO("limit over! filtering...");
+    if(accel < 0)
+      twist.twist.angular.z = (-1) * g_lateral_accel_limit / msg->twist.linear.x;
+    else
+      twist.twist.angular.z = g_lateral_accel_limit / msg->twist.linear.x;
   }
+  else
+  {
+    //ROS_INFO("limit clear!");
+    twist.twist.angular.z = msg->twist.angular.z;
+  }
+
+  ROS_INFO("twist.linear.x = %lf, twist.angular.z = %lf",twist.twist.linear.x,twist.twist.angular.z);
+  g_twist_pub.publish(twist);
 
 }
 
@@ -74,12 +81,11 @@ int main(int argc, char **argv)
 
     ros::NodeHandle nh;
     ros::NodeHandle private_nh("~");
+
     ros::Subscriber twist_sub = nh.subscribe("twist_raw", 1, TwistCmdCallback);
+    ros::Subscriber config_sub = nh.subscribe("config/twist_filter", 10, configCallback);
     g_twist_pub = nh.advertise<geometry_msgs::TwistStamped>("twist_cmd", 1000);
 
     ros::spin();
-
-
-
     return 0;
 }
diff --git a/ros/src/util/packages/runtime_manager/CMakeLists.txt b/ros/src/util/packages/runtime_manager/CMakeLists.txt
@@ -18,6 +18,7 @@ add_message_files(
   ConfigNdtMapping.msg
   ConfigNdtMappingOutput.msg
   ConfigWaypointFollower.msg
+  ConfigTwistFilter.msg
   ConfigVelocitySet.msg
   ConfigCarKf.msg
   ConfigPedestrianKf.msg

diff --git a/ros/src/util/packages/runtime_manager/msg/ConfigTwistFilter.msg b/ros/src/util/packages/runtime_manager/msg/ConfigTwistFilter.msg
@@ -0,0 +1,2 @@
+Header header
+float32 lateral_accel_limit
diff --git a/ros/src/util/packages/runtime_manager/scripts/computing.yaml b/ros/src/util/packages/runtime_manager/scripts/computing.yaml
@@ -265,7 +265,8 @@ subs :
              depend_bool : 'lambda v : v == 0' 
            stat_topic : [ lf ]  
        - name : twist_filter
-         cmd  : rosrun waypoint_follower twist_filter  
+         cmd  : roslaunch waypoint_follower twist_filter.launch
+         param: twist_filter
        - name : wf_simulator
          cmd  : roslaunch waypoint_follower wf_simulator.launch
          param: wf_simulator
@@ -798,6 +799,16 @@ params :
         dash        : ''
         delim       : ':='
 
+  - name  : twist_filter
+    topic : /config/twist_filter
+    msg   : ConfigTwistFilter
+    vars  :
+    - name    : lateral_accel_limit 
+      label   : 'lateral_accel_limit :'
+      min   : 0
+      max   : 5.0
+      v         : 0.8
+
   - name  : wf_simulator
     vars  :
     - name    : use_pose

diff --git a/ros/src/util/packages/runtime_manager/scripts/runtime_manager_dialog.py b/ros/src/util/packages/runtime_manager/scripts/runtime_manager_dialog.py
@@ -64,6 +64,7 @@
 from runtime_manager.msg import ConfigNdtMapping
 from runtime_manager.msg import ConfigNdtMappingOutput
 from runtime_manager.msg import ConfigWaypointFollower
+from runtime_manager.msg import ConfigTwistFilter
 from runtime_manager.msg import ConfigVelocitySet
 from runtime_manager.msg import ConfigCarKf
 from runtime_manager.msg import ConfigPedestrianKf