add scan_filter

CMakeLists.txt

@@ -379,3 +379,11 @@ add_dependencies(teleop_js_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXP
 target_link_libraries(teleop_js_node
   ${catkin_LIBRARIES}
 )
+
+add_executable(lidar_filter
+  src/lidar_filter.cpp
+)
+add_dependencies(lidar_filter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(lidar_filter
+  ${catkin_LIBRARIES}
+)

launch/wpb_scene_1.launch

@@ -40,6 +40,9 @@
   <!-- load the controllers of WPB_HOME -->
   <include file="$(find wpr_simulation)/launch/wpb_home_controllers.launch"/>
 
+  <!-- lidar filter-->
+  <node pkg="wpr_simulation" type="lidar_filter" name="lidar_filter" />
+
   <!-- wpb_home_behaviors -->
   <node pkg="wpb_home_behaviors" type="wpb_home_grab_server" name="wpb_home_grab_server" />
 
@@ -63,8 +66,8 @@
   <node pkg="move_base" type="move_base" respawn="false" name="move_base"  output="screen">
     <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="global_costmap" />
     <rosparam file="$(find wpb_home_tutorials)/nav_lidar/costmap_common_params.yaml" command="load" ns="local_costmap" />
-    <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find wpb_home_tutorials)/nav_lidar/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find wpb_home_tutorials)/nav_lidar/local_planner_params.yaml" command="load" />
     <param name="base_global_planner" value="global_planner/GlobalPlanner" /> 
     <param name="use_dijkstra" value="true"/>

launch/wpb_stage_robocup.launch

@@ -39,6 +39,10 @@
   <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+
+  <!-- lidar filter-->
+  <node pkg="wpr_simulation" type="lidar_filter" name="lidar_filter" />
+
   <!-- joy node -->
   <!-- <node respawn="true" pkg="joy" type="joy_node" name="wpb_home_joy" >
     <param name="dev" type="string" value="/dev/input/js0" />

src/lidar_filter.cpp

@@ -0,0 +1,100 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the WaterPlus nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+/*!******************************************************************
+ @author     ZhangWanjie
+ ********************************************************************/
+
+#include <ros/ros.h>
+#include <std_msgs/String.h>
+#include <sensor_msgs/LaserScan.h>
+
+static std::string pub_topic;
+class CWPHLidarFilter
+{
+public:
+    CWPHLidarFilter();
+private:
+     ros::NodeHandle n;
+     ros::Publisher scan_pub;
+     ros::Subscriber scan_sub;
+     void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
+};
+
+CWPHLidarFilter::CWPHLidarFilter()
+{
+    scan_pub = n.advertise<sensor_msgs::LaserScan>(pub_topic,1);
+    scan_sub = n.subscribe<sensor_msgs::LaserScan>("/scan",1,&CWPHLidarFilter::lidarCallback,this);
+}
+
+void CWPHLidarFilter::lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
+{
+    //ROS_INFO("[wpb_home_lidar_filter]");
+    int nRanges = scan->ranges.size();
+    float k = nRanges/360.0f;
+    sensor_msgs::LaserScan new_scan;
+    new_scan.header.stamp = scan->header.stamp;
+    new_scan.header.frame_id = scan->header.frame_id;
+    new_scan.angle_max = scan->angle_max;
+    new_scan.angle_min = scan->angle_min;
+    // new_scan.angle_increment = scan->angle_increment;
+    new_scan.angle_increment = M_PI/180;
+    // new_scan.time_increment = scan->time_increment;
+    new_scan.time_increment = scan->time_increment*k;
+    new_scan.range_min = 0.25;
+    new_scan.range_max = scan->range_max;
+    new_scan.ranges.resize(360);
+    new_scan.intensities.resize(360);
+    for(int i=0 ; i<360 ; i++)
+    {
+        new_scan.ranges[i] = scan->ranges[i*k];
+        if(new_scan.ranges[i] < 0.25)
+        {
+            new_scan.ranges[i] = new_scan.range_max+1.0;
+        }
+        new_scan.intensities[i] = scan->intensities[i*k];
+    }
+    scan_pub.publish(new_scan);
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc,argv,"lidar_filter");
+
+     ros::NodeHandle n_param("~");
+    std::string strSerialPort;
+    n_param.param<std::string>("pub_topic", pub_topic, "/scan_filtered");
+
+    CWPHLidarFilter lidar_filter;
+    ros::spin();
+}