From 4a4aa57b35a8b21b3c19982e17c524837c979be3 Mon Sep 17 00:00:00 2001 From: Hiroki Ohta Date: Fri, 6 Nov 2015 01:47:02 +0900 Subject: [PATCH 1/5] angular velocity filtering by using lateral acceleration --- .../nodes/twist_filter/twist_filter.cpp | 44 +++++++++---------- 1 file changed, 21 insertions(+), 23 deletions(-) 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..b54b88daaf 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,37 @@ #include #include -#include - -#define LOOP_RATE 100 -#define FILTER_SIZE 3 - //Publisher static ros::Publisher g_twist_pub; - +static double 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 +73,11 @@ int main(int argc, char **argv) ros::NodeHandle nh; ros::NodeHandle private_nh("~"); + + nh.param("lateral_accel_limit",g_lateral_accel_limit,0.8); ros::Subscriber twist_sub = nh.subscribe("twist_raw", 1, TwistCmdCallback); g_twist_pub = nh.advertise("twist_cmd", 1000); ros::spin(); - - - return 0; } From a757b9ecdad01e01178a238da95ef3645b618a56 Mon Sep 17 00:00:00 2001 From: Hiroki Ohta Date: Fri, 6 Nov 2015 04:45:05 +0900 Subject: [PATCH 2/5] created ConfigTwistFilter message Conflicts: ros/src/util/packages/runtime_manager/scripts/computing.yaml --- .../nodes/twist_filter/twist_filter.cpp | 10 +++++++++- .../util/packages/runtime_manager/CMakeLists.txt | 1 + .../packages/runtime_manager/scripts/computing.yaml | 13 ++++++++++++- .../scripts/runtime_manager_dialog.py | 1 + 4 files changed, 23 insertions(+), 2 deletions(-) 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 b54b88daaf..eeaf136268 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 @@ -33,10 +33,18 @@ #include +#include "runtime_manager/ConfigTwistFilter.h" + //Publisher static ros::Publisher g_twist_pub; static double g_lateral_accel_limit; +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; @@ -74,8 +82,8 @@ int main(int argc, char **argv) ros::NodeHandle nh; ros::NodeHandle private_nh("~"); - nh.param("lateral_accel_limit",g_lateral_accel_limit,0.8); 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(); 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/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 From a2cad66892912dabc044ef0a7f1fc6b4f381d87e Mon Sep 17 00:00:00 2001 From: Hiroki Ohta Date: Fri, 6 Nov 2015 15:36:07 +0900 Subject: [PATCH 3/5] added lack things --- .../packages/waypoint_follower/launch/twist_filter.launch | 8 ++++++++ .../packages/runtime_manager/msg/ConfigTwistFilter.msg | 2 ++ 2 files changed, 10 insertions(+) create mode 100644 ros/src/computing/planning/motion/packages/waypoint_follower/launch/twist_filter.launch create mode 100644 ros/src/util/packages/runtime_manager/msg/ConfigTwistFilter.msg 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/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 From 66c2a222366d27021b96484670011090166cfd22 Mon Sep 17 00:00:00 2001 From: pdsljp Date: Fri, 6 Nov 2015 17:44:28 +0900 Subject: [PATCH 4/5] add dependencies --- .../planning/motion/packages/waypoint_follower/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) 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) From 05affa308c2905893af68a9f0ed96783413b7d6f Mon Sep 17 00:00:00 2001 From: Hiroki Ohta Date: Thu, 19 Nov 2015 22:07:27 +0900 Subject: [PATCH 5/5] Add default value --- .../waypoint_follower/nodes/twist_filter/twist_filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 eeaf136268..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 @@ -37,7 +37,7 @@ //Publisher static ros::Publisher g_twist_pub; -static double g_lateral_accel_limit; +static double g_lateral_accel_limit = 0.8; static void configCallback(const runtime_manager::ConfigTwistFilterConstPtr &config) {