diff --git a/wpb_home_bringup/launch/lidar_base.launch b/wpb_home_bringup/launch/lidar_base.launch new file mode 100644 index 0000000..541cb10 --- /dev/null +++ b/wpb_home_bringup/launch/lidar_base.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wpb_home_bringup/launch/normal.launch b/wpb_home_bringup/launch/normal.launch index 519ad69..9caa70a 100644 --- a/wpb_home_bringup/launch/normal.launch +++ b/wpb_home_bringup/launch/normal.launch @@ -1,6 +1,6 @@ - + diff --git a/wpb_home_tutorials/CMakeLists.txt b/wpb_home_tutorials/CMakeLists.txt index 2658ebe..9735735 100644 --- a/wpb_home_tutorials/CMakeLists.txt +++ b/wpb_home_tutorials/CMakeLists.txt @@ -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} +) diff --git a/wpb_home_tutorials/launch/captrue_image.launch b/wpb_home_tutorials/launch/captrue_image.launch new file mode 100644 index 0000000..09bb562 --- /dev/null +++ b/wpb_home_tutorials/launch/captrue_image.launch @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wpb_home_tutorials/launch/face_detect.launch b/wpb_home_tutorials/launch/face_detect.launch index bc6773e..8058d7a 100644 --- a/wpb_home_tutorials/launch/face_detect.launch +++ b/wpb_home_tutorials/launch/face_detect.launch @@ -1,5 +1,5 @@ - + diff --git a/wpb_home_tutorials/launch/follow.launch b/wpb_home_tutorials/launch/follow.launch index c40b612..a2c7891 100644 --- a/wpb_home_tutorials/launch/follow.launch +++ b/wpb_home_tutorials/launch/follow.launch @@ -1,6 +1,6 @@ - + diff --git a/wpb_home_tutorials/launch/gmapping.launch b/wpb_home_tutorials/launch/gmapping.launch index 2d5a413..3499e7a 100644 --- a/wpb_home_tutorials/launch/gmapping.launch +++ b/wpb_home_tutorials/launch/gmapping.launch @@ -1,6 +1,6 @@ - + diff --git a/wpb_home_tutorials/launch/hector_mapping.launch b/wpb_home_tutorials/launch/hector_mapping.launch index f965c58..69d8d98 100644 --- a/wpb_home_tutorials/launch/hector_mapping.launch +++ b/wpb_home_tutorials/launch/hector_mapping.launch @@ -1,6 +1,6 @@ - + diff --git a/wpb_home_tutorials/launch/nav.launch b/wpb_home_tutorials/launch/nav.launch index b5090e1..553bb87 100644 --- a/wpb_home_tutorials/launch/nav.launch +++ b/wpb_home_tutorials/launch/nav.launch @@ -42,7 +42,7 @@ - + diff --git a/wpb_home_tutorials/launch/nav_cruise.launch b/wpb_home_tutorials/launch/nav_cruise.launch index c189e65..009e302 100644 --- a/wpb_home_tutorials/launch/nav_cruise.launch +++ b/wpb_home_tutorials/launch/nav_cruise.launch @@ -42,7 +42,7 @@ - + diff --git a/wpb_home_tutorials/launch/obj_detect.launch b/wpb_home_tutorials/launch/obj_detect.launch index 0d08689..dd3e9fc 100644 --- a/wpb_home_tutorials/launch/obj_detect.launch +++ b/wpb_home_tutorials/launch/obj_detect.launch @@ -1,6 +1,6 @@ - + diff --git a/wpb_home_tutorials/src/wpb_home_capture_image.cpp b/wpb_home_tutorials/src/wpb_home_capture_image.cpp new file mode 100644 index 0000000..853fb7b --- /dev/null +++ b/wpb_home_tutorials/src/wpb_home_capture_image.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include + +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("rgb_topic", rgb_topic, "/camera/image_raw"); + nh_param.param("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; +} \ No newline at end of file diff --git a/wpb_home_tutorials/src/wpb_home_lidar_behavior.cpp b/wpb_home_tutorials/src/wpb_home_lidar_behavior.cpp new file mode 100644 index 0000000..c4c2e10 --- /dev/null +++ b/wpb_home_tutorials/src/wpb_home_lidar_behavior.cpp @@ -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 +#include +#include +#include + +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("/cmd_vel",10); + + ros::spin(); +} diff --git a/wpb_home_tutorials/src/wpb_home_lidar_data.cpp b/wpb_home_tutorials/src/wpb_home_lidar_data.cpp new file mode 100644 index 0000000..892c15b --- /dev/null +++ b/wpb_home_tutorials/src/wpb_home_lidar_data.cpp @@ -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 +#include +#include + +void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan) +{ + int nNum = scan->ranges.size(); + for(int i=0 ; iranges[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(); +} diff --git a/wpbh_local_planner/CMakeLists.txt b/wpbh_local_planner/CMakeLists.txt index 87c8e3c..e22a91b 100644 --- a/wpbh_local_planner/CMakeLists.txt +++ b/wpbh_local_planner/CMakeLists.txt @@ -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 diff --git a/wpbh_local_planner/include/wpbh_local_planner/CLidarAC.h b/wpbh_local_planner/include/wpbh_local_planner/CLidarAC.h new file mode 100644 index 0000000..2440570 --- /dev/null +++ b/wpbh_local_planner/include/wpbh_local_planner/CLidarAC.h @@ -0,0 +1,32 @@ + +#include +#include +#include + +using namespace std; + +class CLidarAC +{ +public: + CLidarAC(); + virtual ~CLidarAC(); + void SetRanges(int* inData); + bool OutLine(); + int GetMinIndex(int inBeginIndex, int inEndIndex); + vector 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; +}; \ No newline at end of file diff --git a/wpbh_local_planner/src/CLidarAC.cpp b/wpbh_local_planner/src/CLidarAC.cpp new file mode 100644 index 0000000..fe40c0c --- /dev/null +++ b/wpbh_local_planner/src/CLidarAC.cpp @@ -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 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= 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; +} \ No newline at end of file diff --git a/wpbh_local_planner/src/wpbh_local_planner.cpp b/wpbh_local_planner/src/wpbh_local_planner.cpp index 9126dd2..b140522 100644 --- a/wpbh_local_planner/src/wpbh_local_planner.cpp +++ b/wpbh_local_planner/src/wpbh_local_planner.cpp @@ -38,10 +38,14 @@ #include #include #include +#include // 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; }