diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt b/ros/src/computing/planning/motion/packages/waypoint_follower/CMakeLists.txt
index 44ebc47e6e..36af611d95 100644
--- 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
new file mode 100644
index 0000000000..20189aab35
--- /dev/null
+++ b/ros/src/computing/planning/motion/packages/waypoint_follower/launch/twist_filter.launch
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
diff --git a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_filter/twist_filter.cpp b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_filter/twist_filter.cpp
index 6fbc4b84dd..9da01000d9 100644
--- a/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_filter/twist_filter.cpp
+++ b/ros/src/computing/planning/motion/packages/waypoint_follower/nodes/twist_filter/twist_filter.cpp
@@ -32,38 +32,45 @@
#include
#include
-#include
-
-#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 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::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("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
index d2a3b76a39..d7ce73a6c2 100644
--- 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
new file mode 100644
index 0000000000..240fda92f8
--- /dev/null
+++ 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
index 23519d2c5a..c119844961 100755
--- 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
index a701e1a064..e029db3af8 100755
--- 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