ROS_Robot/wpb_home
1
0
Fork 0
mirror of https://github.com/6-robot/wpb_home.git synced 2025-09-15 12:58:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

lidar demo

Browse Source
This commit is contained in:
ZhangWanjie 2019-06-25 12:01:46 +08:00
parent 2d1845cc05
commit 3081eef7f6
18 changed files with 689 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
wpb_home_bringup/launch/lidar_base.launch Normal file
View File

@ -0,0 +1,30 @@
<launch>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="true" />
<arg name="rvizconfig" default="$(find wpb_home_bringup)/rviz/sensor.rviz" />
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!--- Run Rplidar -->
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/rplidar"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="laser"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
<!-- wpb_home core-->
<node pkg="wpb_home_bringup" type="wpb_home_core" name="wpb_home_core" output="screen">
<param name="serial_port" type="string" value="/dev/ftdi"/>
<rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
</node>
</launch>

2
wpb_home_bringup/launch/normal.launch
View File

@ -1,6 +1,6 @@
<launch>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

24
wpb_home_tutorials/CMakeLists.txt
View File

@ -301,3 +301,27 @@ add_dependencies(wpb_home_grab_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catk
target_link_libraries(wpb_home_grab_client
${catkin_LIBRARIES}
)
add_executable(wpb_home_capture_image
src/wpb_home_capture_image.cpp
)
add_dependencies(wpb_home_capture_image ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_capture_image
${catkin_LIBRARIES}
)
add_executable(wpb_home_lidar_data
src/wpb_home_lidar_data.cpp
)
add_dependencies(wpb_home_lidar_data ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_lidar_data
${catkin_LIBRARIES}
)
add_executable(wpb_home_lidar_behavior
src/wpb_home_lidar_behavior.cpp
)
add_dependencies(wpb_home_lidar_behavior ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_lidar_behavior
${catkin_LIBRARIES}
)

111
wpb_home_tutorials/launch/captrue_image.launch Normal file
View File

@ -0,0 +1,111 @@
<launch>
<!-- wpb_home core-->
<node pkg="wpb_home_bringup" type="wpb_home_core" name="wpb_home_core" output="screen">
<param name="serial_port" type="string" value="/dev/ftdi"/>
<rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
</node>
<!-- js node -->
<node respawn="true" pkg="joy" type="joy_node" name="wpb_home_joy" >
<param name="dev" type="string" value="/dev/input/js0" />
<param name="deadzone" value="0.12" />
</node>
<!-- axes velcmd -->
<param name="axis_linear" value="1" type="int"/>
<param name="axis_angular" value="0" type="int"/>
<param name="scale_linear" value="1" type="double"/>
<param name="scale_angular" value="1" type="double"/>
<node pkg="wpb_home_bringup" type="wpb_home_js_vel" name="teleop"/>
<!-- capture image -->
<node pkg="wpb_home_tutorials" type="wpb_home_capture_image" name="wpb_home_capture_image" output="screen"/>
<!--************************ kinect ************************-->
<arg name="base_name" default="kinect2"/>
<arg name="sensor" default=""/>
<arg name="publish_tf" default="false"/>
<arg name="base_name_tf" default="$(arg base_name)"/>
<arg name="fps_limit" default="-1.0"/>
<arg name="calib_path" default="$(find kinect2_bridge)/data/"/>
<arg name="use_png" default="false"/>
<arg name="jpeg_quality" default="90"/>
<arg name="png_level" default="1"/>
<arg name="depth_method" default="cpu"/>
<arg name="depth_device" default="-1"/>
<arg name="reg_method" default="cpu"/>
<arg name="reg_device" default="-1"/>
<arg name="max_depth" default="12.0"/>
<arg name="min_depth" default="0.1"/>
<arg name="queue_size" default="5"/>
<arg name="bilateral_filter" default="true"/>
<arg name="edge_aware_filter" default="true"/>
<arg name="worker_threads" default="4"/>
<arg name="machine" default="localhost"/>
<arg name="nodelet_manager" default="$(arg base_name)"/>
<arg name="start_manager" default="true"/>
<arg name="use_machine" default="true"/>
<arg name="respawn" default="true"/>
<arg name="use_nodelet" default="true"/>
<arg name="output" default="screen"/>
<machine name="localhost" address="localhost" if="$(arg use_machine)"/>
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager"
if="$(arg start_manager)" machine="$(arg machine)" />
<!-- Nodelet version of kinect2_bridge -->
<node pkg="nodelet" type="nodelet" name="$(arg base_name)_bridge" machine="$(arg machine)"
args="load kinect2_bridge/kinect2_bridge_nodelet $(arg nodelet_manager)"
respawn="$(arg respawn)" output="$(arg output)" if="$(arg use_nodelet)">
<param name="base_name" type="str" value="$(arg base_name)"/>
<param name="sensor" type="str" value="$(arg sensor)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf)"/>
<param name="base_name_tf" type="str" value="$(arg base_name_tf)"/>
<param name="fps_limit" type="double" value="$(arg fps_limit)"/>
<param name="calib_path" type="str" value="$(arg calib_path)"/>
<param name="use_png" type="bool" value="$(arg use_png)"/>
<param name="jpeg_quality" type="int" value="$(arg jpeg_quality)"/>
<param name="png_level" type="int" value="$(arg png_level)"/>
<param name="depth_method" type="str" value="$(arg depth_method)"/>
<param name="depth_device" type="int" value="$(arg depth_device)"/>
<param name="reg_method" type="str" value="$(arg reg_method)"/>
<param name="reg_device" type="int" value="$(arg reg_device)"/>
<param name="max_depth" type="double" value="$(arg max_depth)"/>
<param name="min_depth" type="double" value="$(arg min_depth)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="bilateral_filter" type="bool" value="$(arg bilateral_filter)"/>
<param name="edge_aware_filter" type="bool" value="$(arg edge_aware_filter)"/>
<param name="worker_threads" type="int" value="$(arg worker_threads)"/>
</node>
<!-- Node version of kinect2_bridge -->
<node pkg="kinect2_bridge" type="kinect2_bridge" name="$(arg base_name)_bridge" machine="$(arg machine)"
respawn="$(arg respawn)" output="$(arg output)" unless="$(arg use_nodelet)">
<param name="base_name" type="str" value="$(arg base_name)"/>
<param name="sensor" type="str" value="$(arg sensor)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf)"/>
<param name="base_name_tf" type="str" value="$(arg base_name_tf)"/>
<param name="fps_limit" type="double" value="$(arg fps_limit)"/>
<param name="calib_path" type="str" value="$(arg calib_path)"/>
<param name="use_png" type="bool" value="$(arg use_png)"/>
<param name="jpeg_quality" type="int" value="$(arg jpeg_quality)"/>
<param name="png_level" type="int" value="$(arg png_level)"/>
<param name="depth_method" type="str" value="$(arg depth_method)"/>
<param name="depth_device" type="int" value="$(arg depth_device)"/>
<param name="reg_method" type="str" value="$(arg reg_method)"/>
<param name="reg_device" type="int" value="$(arg reg_device)"/>
<param name="max_depth" type="double" value="$(arg max_depth)"/>
<param name="min_depth" type="double" value="$(arg min_depth)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="bilateral_filter" type="bool" value="$(arg bilateral_filter)"/>
<param name="edge_aware_filter" type="bool" value="$(arg edge_aware_filter)"/>
<param name="worker_threads" type="int" value="$(arg worker_threads)"/>
</node>
<!--************************ end ************************-->
</launch>

2
wpb_home_tutorials/launch/face_detect.launch
View File

@ -1,5 +1,5 @@
<launch>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="true" />
<arg name="rvizconfig" default="$(find wpb_home_tutorials)/rviz/face_detect.rviz" />

2
wpb_home_tutorials/launch/follow.launch
View File

@ -1,6 +1,6 @@
<launch>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="true" />
<arg name="rvizconfig" default="$(find wpb_home_tutorials)/rviz/follow.rviz" />

2
wpb_home_tutorials/launch/gmapping.launch
View File

@ -1,6 +1,6 @@
<launch>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="true" />
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />

2
wpb_home_tutorials/launch/hector_mapping.launch
View File

@ -1,6 +1,6 @@
<launch>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="true" />
<arg name="rvizconfig" default="$(find wpb_home_tutorials)/rviz/slam.rviz" />

2
wpb_home_tutorials/launch/nav.launch
View File

@ -42,7 +42,7 @@
</node>
<!-- RViz and TF tree -->
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="true" />
<arg name="rvizconfig" default="$(find wpb_home_tutorials)/rviz/nav.rviz" />

2
wpb_home_tutorials/launch/nav_cruise.launch
View File

@ -42,7 +42,7 @@
</node>
<!-- RViz and TF tree -->
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="true" />
<arg name="rvizconfig" default="$(find wpb_home_tutorials)/rviz/nav.rviz" />

2
wpb_home_tutorials/launch/obj_detect.launch
View File

@ -1,6 +1,6 @@
<launch>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home_mani.urdf"/>
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="true" />
<arg name="rvizconfig" default="$(find wpb_home_tutorials)/rviz/obj_detect.rviz" />

122
wpb_home_tutorials/src/wpb_home_capture_image.cpp Normal file
View File

@ -0,0 +1,122 @@
/*********************************************************************
* 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 <unistd.h>
#include <sensor_msgs/Joy.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;
static std::string rgb_topic;
static const std::string WIN_TITLE = "image";
static ros::Publisher image_pub;
static char filename[128];
static int nImageIndex = 1;
static bool bCaptrueOneFrame = false;
static char ImageFlag[] = "Drink";
void callbackRGB(const sensor_msgs::ImageConstPtr& msg)
{
//ROS_INFO("callbackRGB");
if(bCaptrueOneFrame == true)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
char *home_path = getenv("HOME");
sprintf(filename,"%s/Capture_Images/%s_%04d.jpg",home_path,ImageFlag,nImageIndex);
while (access(filename,R_OK) == 0)
{
nImageIndex++;
sprintf(filename,"%s/Capture_Images/%s_%04d.jpg",home_path,ImageFlag,nImageIndex);
}
imwrite(filename,cv_ptr->image);
ROS_WARN("captrue %s",filename);
bCaptrueOneFrame = false;
}
}
void callbackJoy(const sensor_msgs::Joy::ConstPtr& joy)
{
int btn_x = joy->buttons[2];
//ROS_WARN("btn x = %d",btn_x);
if(btn_x > 0)
{
bCaptrueOneFrame = true;
}
}
int main(int argc, char **argv)
{
cv::namedWindow(WIN_TITLE);
ros::init(argc, argv, "wpb_home_capture_image");
ros::NodeHandle nh_param("~");
//nh_param.param<std::string>("rgb_topic", rgb_topic, "/camera/image_raw");
nh_param.param<std::string>("rgb_topic", rgb_topic, "/kinect2/hd/image_color");
ROS_WARN("wpb_home_capture_image");
ros::NodeHandle nh;
ros::Subscriber rgb_sub = nh.subscribe(rgb_topic, 1 , callbackRGB);
ros::Subscriber joy_sub = nh.subscribe("joy", 10 , callbackJoy);
ros::Rate loop_rate(1);
while( ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
cv::destroyWindow(WIN_TITLE);
return 0;
}

76
wpb_home_tutorials/src/wpb_home_lidar_behavior.cpp Normal file
View File

@ -0,0 +1,76 @@
/*********************************************************************
* 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>
#include <geometry_msgs/Twist.h>
ros::Publisher vel_pub;
void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
int nNum = scan->ranges.size();
int nMid = nNum/2;
float fMidDist = scan->ranges[nMid];
ROS_INFO("Point[%d] = %f", nMid, fMidDist);
geometry_msgs::Twist vel_cmd;
if(fMidDist > 1.0f)
{
vel_cmd.linear.x = 0.05;
}
else
{
vel_cmd.angular.z = 0.1;
}
vel_pub.publish(vel_cmd);
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"wpb_home_lidar_behavior");
ROS_INFO("wpb_home_lidar_behavior start!");
ros::NodeHandle nh;
ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &lidarCallback);
vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel",10);
ros::spin();
}

61
wpb_home_tutorials/src/wpb_home_lidar_data.cpp Normal file
View File

@ -0,0 +1,61 @@
/*********************************************************************
* 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>
void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
int nNum = scan->ranges.size();
for(int i=0 ; i<nNum ; i++)
{
ROS_INFO("Point[%d] = %f", i, scan->ranges[i]);
}
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"wpb_home_lidar_data");
ROS_INFO("wpb_home_lidar_data start!");
ros::NodeHandle nh;
ros::Subscriber lidar_sub = nh.subscribe("/scan", 10, &lidarCallback);
ros::spin();
}

2
wpbh_local_planner/CMakeLists.txt
View File

@ -44,7 +44,9 @@ install(FILES
add_library(wpbh_local_planner
include/wpbh_local_planner/wpbh_local_planner.h
include/wpbh_local_planner/CLidarAC.h
src/wpbh_local_planner.cpp
src/CLidarAC.cpp
)
add_dependencies(wpbh_local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpbh_local_planner

32
wpbh_local_planner/include/wpbh_local_planner/CLidarAC.h Normal file
View File

@ -0,0 +1,32 @@
#include <math.h>
#include <stdlib.h>
#include <vector>
using namespace std;
class CLidarAC
{
public:
CLidarAC();
virtual ~CLidarAC();
void SetRanges(int* inData);
bool OutLine();
int GetMinIndex(int inBeginIndex, int inEndIndex);
vector<int> arBlobIndex;
int arFrontRanges[180];
int arPnt_x[180];
int arPnt_y[180];
bool arACPnt[180];
int nACWidth;
int nACDist;
int nIgnDist;
int nBlobDist;
int nLeftIndex;
int nRightIndex;
float kx;
float ky;
float fVx;
float fVy;
};

204
wpbh_local_planner/src/CLidarAC.cpp Normal file
View File

@ -0,0 +1,204 @@
#include "wpbh_local_planner/CLidarAC.h"
static double y_sin[180];
static double x_cos[180];
static float max_vy = 0.5;
CLidarAC::CLidarAC()
{
for(int i=0;i<180;i++)
{
arFrontRanges[i] = i;
x_cos[i] = cos(i*3.14159/180);
y_sin[i] = sin(i*3.14159/180);
arPnt_x[i] = arFrontRanges[i] * x_cos[i];
arPnt_y[i] = arFrontRanges[i] * y_sin[i];
arACPnt[i] = false;
}
kx = 0.005;
ky = 0.1;
fVx = 0;
fVy = 0;
nLeftIndex = nRightIndex = -1;
nBlobDist = 5;
nIgnDist = 22;
nACWidth = 30;
nACDist = 100;
}
CLidarAC::~CLidarAC()
{
}
static int CalDist(int inX1, int inY1, int inX2, int inY2)
{
int res = sqrt((inX1-inX2)*(inX1-inX2) + (inY1-inY2)*(inY1-inY2));
return res;
}
int CLidarAC::GetMinIndex(int inBeginIndex, int inEndIndex)
{
int res = inBeginIndex;
int nMinDist = arFrontRanges[inBeginIndex];
for(int i=inBeginIndex+1;i<inEndIndex;i++)
{
if(arACPnt[i] == true && arFrontRanges[i] < nMinDist)
{
nMinDist = arFrontRanges[i];
res = i;
}
}
return res;
}
void CLidarAC::SetRanges(int* inData)
{
for(int i=0;i<180;i++)
{
arFrontRanges[i] = inData[270-i];
arPnt_x[i] = arFrontRanges[i] * x_cos[i];
arPnt_y[i] = arFrontRanges[i] * y_sin[i];
}
}
bool CLidarAC::OutLine()
{
bool res = true;
// 有效障碍点
for(int i=0;i<180;i++)
{
arACPnt[i] = false;
if(arFrontRanges[i] > nIgnDist )
{
if(
arPnt_x[i] > -nACWidth &&
arPnt_x[i] < nACWidth &&
arPnt_y[i] > 0 &&
arPnt_y[i] < nACDist
)
{
arACPnt[i] = true;
}
}
}
// 连通检测
bool bBlobStart = false;
int nBeginIndex = 0;
int nEndIndex = 0;
int nLastRange = 0;
arBlobIndex.clear();
for(int i=0;i<180;i++)
{
if(arACPnt[i] == true)
{
if(bBlobStart == false)
{
nBeginIndex = i;
bBlobStart = true;
}
else
{
if(abs(arFrontRanges[i] - nLastRange) > nBlobDist)
{
nEndIndex = i-1;
if(nEndIndex > nBeginIndex)
{
int nMinIndex = GetMinIndex(nBeginIndex , nEndIndex);
arBlobIndex.push_back(nMinIndex);
}
nBeginIndex = i;
}
}
nLastRange = arFrontRanges[i];
}
else
{
if(bBlobStart == true)
{
bBlobStart= false;
nEndIndex = i-1;
if(nEndIndex > nBeginIndex)
{
int nMinIndex = GetMinIndex(nBeginIndex , nEndIndex);
arBlobIndex.push_back(nMinIndex);
}
}
}
}
//左右
nLeftIndex = nRightIndex = -1;
int nMinLeft = 1000;
int nMinRight = 1000;
int nBlobNum = arBlobIndex.size();
for(int i=0;i<nBlobNum;i++)
{
if(arBlobIndex[i] < 90)
{
if(arFrontRanges[arBlobIndex[i]] < nMinLeft)
{
nMinLeft = arFrontRanges[arBlobIndex[i]];
nLeftIndex = arBlobIndex[i];
}
}
else
{
if(arFrontRanges[arBlobIndex[i]] < nMinRight)
{
nMinRight = arFrontRanges[arBlobIndex[i]];
nRightIndex = arBlobIndex[i];
}
}
}
// 速度
if(nLeftIndex < 0 && nRightIndex < 0)
{
fVx = fVy = 0;
}
else
{
int nMinX = nACDist;
if(nLeftIndex >= 0 )
{
if(arPnt_y[nLeftIndex] < nMinX)
{
nMinX = arPnt_y[nLeftIndex];
}
}
if(nRightIndex >= 0 )
{
if(arPnt_y[nRightIndex] < nMinX)
{
nMinX = arPnt_y[nRightIndex];
}
}
fVx = -(nACDist - nMinX)*kx;
if(nMinX < 40)
{
res = false;
}
float tmpLeftV = 0;
float tmpRightV = 0;
if(nLeftIndex >= 0 )
{
tmpLeftV = (arPnt_x[nLeftIndex] + nACWidth) * ky / arPnt_y[nLeftIndex];
}
if(nRightIndex >= 0 )
{
tmpRightV = -(nACWidth - arPnt_x[nRightIndex]) * ky / arPnt_y[nRightIndex];
}
fVy = tmpLeftV + tmpRightV;
if(fVy < -max_vy)
{
fVy = -max_vy;
}
if(fVy > max_vy)
{
fVy = max_vy;
}
}
return res;
}

86
wpbh_local_planner/src/wpbh_local_planner.cpp
View File

@ -38,10 +38,14 @@
#include <wpbh_local_planner/wpbh_local_planner.h>
#include <tf_conversions/tf_eigen.h>
#include <pluginlib/class_list_macros.h>
#include <wpbh_local_planner/CLidarAC.h>
// register this planner as a WpbhLocalPlanner plugin
PLUGINLIB_DECLARE_CLASS(wpbh_local_planner, WpbhLocalPlanner, wpbh_local_planner::WpbhLocalPlanner, nav_core::BaseLocalPlanner)
static CLidarAC lidar_ac;
static int ranges[360];
namespace wpbh_local_planner
{
WpbhLocalPlanner::WpbhLocalPlanner()
@ -100,63 +104,9 @@ namespace wpbh_local_planner
void WpbhLocalPlanner::lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
//ROS_INFO("WpbhLocalPlanner::lidarCallback");
float left_obst = 1.0;
float right_obst = 1.0;
int nRanges = scan->ranges.size();
float range_max = scan->range_max;
float range_min = scan->range_min;
float ac_width = 0.20;
float ac_dist = 0.45;
//right front
for(int i=90;i<180;i++)
for(int i=0;i<360;i++)
{
if(scan->ranges[i] < range_min || scan->ranges[i] > range_max)
continue;
float dist = scan->ranges[i] * sin((i-90)*fScaleD2R);
if(dist < ac_dist)
{
float width = scan->ranges[i] * cos((i-90)*fScaleD2R);
if(width < right_obst)
{
right_obst = width;
}
}
}
//left front
for(int i=180;i<270;i++)
{
if(scan->ranges[i] < range_min || scan->ranges[i] > range_max)
continue;
float dist = scan->ranges[i] * sin((270-i)*fScaleD2R);
if(dist < ac_dist)
{
float width = scan->ranges[i] * cos((270-i)*fScaleD2R);
if(width < left_obst)
{
left_obst = width;
}
}
}
// ajust
if(right_obst > ac_width && left_obst > ac_width)
{
m_bAC = false;
}
else
{
m_bAC = true;
if(right_obst < left_obst)
{
//m_nAC_action = AC_TURN_LEFT;
}
else
{
//m_nAC_action = AC_TURN_RIGHT;
}
ranges[i] = scan->ranges[i]*100;
}
}
@ -169,6 +119,11 @@ namespace wpbh_local_planner
return false;
}
for(int i=0;i<360;i++)
{
ranges[i] = 0;
}
m_global_plan.clear();
m_global_plan = plan;
m_nPathIndex = 0;
@ -256,6 +211,7 @@ namespace wpbh_local_planner
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
cmd_vel.angular.z = 0;
bool res = true;
/////////////////////////////////////////////////
if(m_bFirstStep == true)
@ -353,21 +309,17 @@ namespace wpbh_local_planner
cmd_vel.linear.x = target_dist*cos(face_target) * m_acc_scale_trans;
cmd_vel.linear.y = target_dist*sin(face_target) * m_acc_scale_trans;
cmd_vel.angular.z = face_target * m_acc_scale_rot;
///////////////////
// lidar_ac.SetRanges(ranges);
// res = lidar_ac.OutLine();
// cmd_vel.linear.x += lidar_ac.fVx;
// cmd_vel.linear.y += lidar_ac.fVy;
///////////////////
if(cmd_vel.linear.x > 0) cmd_vel.linear.x+=0.05;
if(cmd_vel.linear.x < 0) cmd_vel.linear.x-=0.05;
if(cmd_vel.linear.y > 0) cmd_vel.linear.y+=0.02;
if(cmd_vel.linear.y < 0) cmd_vel.linear.y-=0.02;
// cmd_vel.linear.x = 0;
// cmd_vel.linear.y = 0;
if(m_bAC == true)
{
m_bAC = false;
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
cmd_vel.angular.z = 0;
return m_bAC;
}
}
m_pub_target.publish(m_global_plan[m_nPathIndex]);
}
@ -399,7 +351,7 @@ namespace wpbh_local_planner
m_last_cmd = cmd_vel;
return true;
return res;
}