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

first commit

Browse Source
This commit is contained in:
Robot 2024-10-10 17:53:37 +08:00
commit 88224d13ec
8 changed files with 1279 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
.gitignore vendored Normal file
View File

@ -0,0 +1,28 @@
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app

222
CMakeLists.txt Normal file
View File

@ -0,0 +1,222 @@
cmake_minimum_required(VERSION 3.0.2)
project(jie_ware)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
cv_bridge
geometry_msgs
move_base_msgs
nav_msgs
roscpp
rospy
sensor_msgs
std_msgs
tf
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(OpenCV REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# move_base_msgs# nav_msgs# sensor_msgs# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES jie_ware
CATKIN_DEPENDS cv_bridge geometry_msgs move_base_msgs nav_msgs roscpp rospy sensor_msgs std_msgs tf
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/jie_ware.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/jie_ware_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_jie_ware.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
add_executable(lidar_loc
src/lidar_loc.cpp
)
add_dependencies(lidar_loc ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(lidar_loc
${catkin_LIBRARIES}
${OpenCV_LIBS}
)

339
LICENSE Normal file
View File

@ -0,0 +1,339 @@
GNU GENERAL PUBLIC LICENSE
Version 2, June 1991
Copyright (C) 1989, 1991 Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
Preamble
The licenses for most software are designed to take away your
freedom to share and change it. By contrast, the GNU General Public
License is intended to guarantee your freedom to share and change free
software--to make sure the software is free for all its users. This
General Public License applies to most of the Free Software
Foundation's software and to any other program whose authors commit to
using it. (Some other Free Software Foundation software is covered by
the GNU Lesser General Public License instead.) You can apply it to
your programs, too.
When we speak of free software, we are referring to freedom, not
price. Our General Public Licenses are designed to make sure that you
have the freedom to distribute copies of free software (and charge for
this service if you wish), that you receive source code or can get it
if you want it, that you can change the software or use pieces of it
in new free programs; and that you know you can do these things.
To protect your rights, we need to make restrictions that forbid
anyone to deny you these rights or to ask you to surrender the rights.
These restrictions translate to certain responsibilities for you if you
distribute copies of the software, or if you modify it.
For example, if you distribute copies of such a program, whether
gratis or for a fee, you must give the recipients all the rights that
you have. You must make sure that they, too, receive or can get the
source code. And you must show them these terms so they know their
rights.
We protect your rights with two steps: (1) copyright the software, and
(2) offer you this license which gives you legal permission to copy,
distribute and/or modify the software.
Also, for each author's protection and ours, we want to make certain
that everyone understands that there is no warranty for this free
software. If the software is modified by someone else and passed on, we
want its recipients to know that what they have is not the original, so
that any problems introduced by others will not reflect on the original
authors' reputations.
Finally, any free program is threatened constantly by software
patents. We wish to avoid the danger that redistributors of a free
program will individually obtain patent licenses, in effect making the
program proprietary. To prevent this, we have made it clear that any
patent must be licensed for everyone's free use or not licensed at all.
The precise terms and conditions for copying, distribution and
modification follow.
GNU GENERAL PUBLIC LICENSE
TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
0. This License applies to any program or other work which contains
a notice placed by the copyright holder saying it may be distributed
under the terms of this General Public License. The "Program", below,
refers to any such program or work, and a "work based on the Program"
means either the Program or any derivative work under copyright law:
that is to say, a work containing the Program or a portion of it,
either verbatim or with modifications and/or translated into another
language. (Hereinafter, translation is included without limitation in
the term "modification".) Each licensee is addressed as "you".
Activities other than copying, distribution and modification are not
covered by this License; they are outside its scope. The act of
running the Program is not restricted, and the output from the Program
is covered only if its contents constitute a work based on the
Program (independent of having been made by running the Program).
Whether that is true depends on what the Program does.
1. You may copy and distribute verbatim copies of the Program's
source code as you receive it, in any medium, provided that you
conspicuously and appropriately publish on each copy an appropriate
copyright notice and disclaimer of warranty; keep intact all the
notices that refer to this License and to the absence of any warranty;
and give any other recipients of the Program a copy of this License
along with the Program.
You may charge a fee for the physical act of transferring a copy, and
you may at your option offer warranty protection in exchange for a fee.
2. You may modify your copy or copies of the Program or any portion
of it, thus forming a work based on the Program, and copy and
distribute such modifications or work under the terms of Section 1
above, provided that you also meet all of these conditions:
a) You must cause the modified files to carry prominent notices
stating that you changed the files and the date of any change.
b) You must cause any work that you distribute or publish, that in
whole or in part contains or is derived from the Program or any
part thereof, to be licensed as a whole at no charge to all third
parties under the terms of this License.
c) If the modified program normally reads commands interactively
when run, you must cause it, when started running for such
interactive use in the most ordinary way, to print or display an
announcement including an appropriate copyright notice and a
notice that there is no warranty (or else, saying that you provide
a warranty) and that users may redistribute the program under
these conditions, and telling the user how to view a copy of this
License. (Exception: if the Program itself is interactive but
does not normally print such an announcement, your work based on
the Program is not required to print an announcement.)
These requirements apply to the modified work as a whole. If
identifiable sections of that work are not derived from the Program,
and can be reasonably considered independent and separate works in
themselves, then this License, and its terms, do not apply to those
sections when you distribute them as separate works. But when you
distribute the same sections as part of a whole which is a work based
on the Program, the distribution of the whole must be on the terms of
this License, whose permissions for other licensees extend to the
entire whole, and thus to each and every part regardless of who wrote it.
Thus, it is not the intent of this section to claim rights or contest
your rights to work written entirely by you; rather, the intent is to
exercise the right to control the distribution of derivative or
collective works based on the Program.
In addition, mere aggregation of another work not based on the Program
with the Program (or with a work based on the Program) on a volume of
a storage or distribution medium does not bring the other work under
the scope of this License.
3. You may copy and distribute the Program (or a work based on it,
under Section 2) in object code or executable form under the terms of
Sections 1 and 2 above provided that you also do one of the following:
a) Accompany it with the complete corresponding machine-readable
source code, which must be distributed under the terms of Sections
1 and 2 above on a medium customarily used for software interchange; or,
b) Accompany it with a written offer, valid for at least three
years, to give any third party, for a charge no more than your
cost of physically performing source distribution, a complete
machine-readable copy of the corresponding source code, to be
distributed under the terms of Sections 1 and 2 above on a medium
customarily used for software interchange; or,
c) Accompany it with the information you received as to the offer
to distribute corresponding source code. (This alternative is
allowed only for noncommercial distribution and only if you
received the program in object code or executable form with such
an offer, in accord with Subsection b above.)
The source code for a work means the preferred form of the work for
making modifications to it. For an executable work, complete source
code means all the source code for all modules it contains, plus any
associated interface definition files, plus the scripts used to
control compilation and installation of the executable. However, as a
special exception, the source code distributed need not include
anything that is normally distributed (in either source or binary
form) with the major components (compiler, kernel, and so on) of the
operating system on which the executable runs, unless that component
itself accompanies the executable.
If distribution of executable or object code is made by offering
access to copy from a designated place, then offering equivalent
access to copy the source code from the same place counts as
distribution of the source code, even though third parties are not
compelled to copy the source along with the object code.
4. You may not copy, modify, sublicense, or distribute the Program
except as expressly provided under this License. Any attempt
otherwise to copy, modify, sublicense or distribute the Program is
void, and will automatically terminate your rights under this License.
However, parties who have received copies, or rights, from you under
this License will not have their licenses terminated so long as such
parties remain in full compliance.
5. You are not required to accept this License, since you have not
signed it. However, nothing else grants you permission to modify or
distribute the Program or its derivative works. These actions are
prohibited by law if you do not accept this License. Therefore, by
modifying or distributing the Program (or any work based on the
Program), you indicate your acceptance of this License to do so, and
all its terms and conditions for copying, distributing or modifying
the Program or works based on it.
6. Each time you redistribute the Program (or any work based on the
Program), the recipient automatically receives a license from the
original licensor to copy, distribute or modify the Program subject to
these terms and conditions. You may not impose any further
restrictions on the recipients' exercise of the rights granted herein.
You are not responsible for enforcing compliance by third parties to
this License.
7. If, as a consequence of a court judgment or allegation of patent
infringement or for any other reason (not limited to patent issues),
conditions are imposed on you (whether by court order, agreement or
otherwise) that contradict the conditions of this License, they do not
excuse you from the conditions of this License. If you cannot
distribute so as to satisfy simultaneously your obligations under this
License and any other pertinent obligations, then as a consequence you
may not distribute the Program at all. For example, if a patent
license would not permit royalty-free redistribution of the Program by
all those who receive copies directly or indirectly through you, then
the only way you could satisfy both it and this License would be to
refrain entirely from distribution of the Program.
If any portion of this section is held invalid or unenforceable under
any particular circumstance, the balance of the section is intended to
apply and the section as a whole is intended to apply in other
circumstances.
It is not the purpose of this section to induce you to infringe any
patents or other property right claims or to contest validity of any
such claims; this section has the sole purpose of protecting the
integrity of the free software distribution system, which is
implemented by public license practices. Many people have made
generous contributions to the wide range of software distributed
through that system in reliance on consistent application of that
system; it is up to the author/donor to decide if he or she is willing
to distribute software through any other system and a licensee cannot
impose that choice.
This section is intended to make thoroughly clear what is believed to
be a consequence of the rest of this License.
8. If the distribution and/or use of the Program is restricted in
certain countries either by patents or by copyrighted interfaces, the
original copyright holder who places the Program under this License
may add an explicit geographical distribution limitation excluding
those countries, so that distribution is permitted only in or among
countries not thus excluded. In such case, this License incorporates
the limitation as if written in the body of this License.
9. The Free Software Foundation may publish revised and/or new versions
of the General Public License from time to time. Such new versions will
be similar in spirit to the present version, but may differ in detail to
address new problems or concerns.
Each version is given a distinguishing version number. If the Program
specifies a version number of this License which applies to it and "any
later version", you have the option of following the terms and conditions
either of that version or of any later version published by the Free
Software Foundation. If the Program does not specify a version number of
this License, you may choose any version ever published by the Free Software
Foundation.
10. If you wish to incorporate parts of the Program into other free
programs whose distribution conditions are different, write to the author
to ask for permission. For software which is copyrighted by the Free
Software Foundation, write to the Free Software Foundation; we sometimes
make exceptions for this. Our decision will be guided by the two goals
of preserving the free status of all derivatives of our free software and
of promoting the sharing and reuse of software generally.
NO WARRANTY
11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN
OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS
TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE
PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
REPAIR OR CORRECTION.
12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
POSSIBILITY OF SUCH DAMAGES.
END OF TERMS AND CONDITIONS
How to Apply These Terms to Your New Programs
If you develop a new program, and you want it to be of the greatest
possible use to the public, the best way to achieve this is to make it
free software which everyone can redistribute and change under these terms.
To do so, attach the following notices to the program. It is safest
to attach them to the start of each source file to most effectively
convey the exclusion of warranty; and each file should have at least
the "copyright" line and a pointer to where the full notice is found.
<one line to give the program's name and a brief idea of what it does.>
Copyright (C) <year> <name of author>
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
Also add information on how to contact you by electronic and paper mail.
If the program is interactive, make it output a short notice like this
when it starts in an interactive mode:
Gnomovision version 69, Copyright (C) year name of author
Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
This is free software, and you are welcome to redistribute it
under certain conditions; type `show c' for details.
The hypothetical commands `show w' and `show c' should show the appropriate
parts of the General Public License. Of course, the commands you use may
be called something other than `show w' and `show c'; they could even be
mouse-clicks or menu items--whatever suits your program.
You should also get your employer (if you work as a programmer) or your
school, if any, to sign a "copyright disclaimer" for the program, if
necessary. Here is a sample; alter the names:
Yoyodyne, Inc., hereby disclaims all copyright interest in the program
`Gnomovision' (which makes passes at compilers) written by James Hacker.
<signature of Ty Coon>, 1 April 1989
Ty Coon, President of Vice
This General Public License does not permit incorporating your program into
proprietary programs. If your program is a subroutine library, you may
consider it more useful to permit linking proprietary applications with the
library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License.

29
README.md Normal file
View File

@ -0,0 +1,29 @@
# JIE Ware ROS 工具集
## 使用步骤
1. 获取源码:
```
cd ~/catkin_ws/src/
git clone https://github.com/6-robot/jie_ware.git
```
2. 编译
```
cd ~/catkin_ws
catkin_make
```
3. 修改Launch文件，替换AMCL
~~<node pkg="amcl" type="amcl" name="amcl">~~
~~ ......~~
~~</node>~~
```
<node pkg="jie_ware" type="lidar_loc" name="lidar_loc" >
<param name="base_frame" value="base_footprint" />
<param name="laser_frame" value="laser" />
</node>
```
4. 运行修改后的Launch文件
```
roslaunch jie_ware lidar_loc_test.launch
```

75
launch/amcl_test.launch Normal file
View File

@ -0,0 +1,75 @@
<launch>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find wpr_simulation)/worlds/robocup_home.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="recording" value="false"/>
<arg name="debug" value="false"/>
</include>
<!-- Spawn the objects into Gazebo -->
<node name="bed" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bed.model -x 5.0 -y -3.9 -z 0 -Y 3.14159 -urdf -model bed" />
<node name="sofa" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/sofa.model -x -1.0 -y -3.9 -z 0 -Y 1.57 -urdf -model sofa" />
<node name="tea_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/tea_table.model -x -2.1 -y -2.2 -z 0 -Y 1.57 -urdf -model tea_table" />
<node name="bookshelft" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bookshelft.model -x 2.0 -y -0.55 -z 0 -Y -1.57 -urdf -model bookshelft" />
<node name="kitchen_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x -3.5 -y 3.7 -z 0 -Y 1.57 -urdf -model kitchen_table" />
<node name="cupboard_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -2.0 -y 0.7 -z 0 -Y 1.57 -urdf -model cupboard_0" />
<node name="cupboard_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -1.3 -y 3.7 -z 0 -Y -1.57 -urdf -model cupboard_1" />
<node name="dinning_table_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_0" />
<node name="dinning_table_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_1" />
<node name="dinning_table_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_2" />
<node name="dinning_table_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_3" />
<node name="chair_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_0" />
<node name="chair_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_1" />
<node name="chair_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_2" />
<node name="chair_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_3" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home.model -urdf -x -0.0 -y -0.0 -model wpb_home" />
<!-- Run the map server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
<!--- Run AMCL -->
<include file="$(find wpb_home_tutorials)/nav_lidar/amcl_omni.launch" />
<!--- Run move base -->
<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_planner_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="use_dijkstra" value="true"/>
<param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
<param name= "controller_frequency" value="10" type="double"/>
</node>
<!-- RViz and TF tree -->
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="false" />
<arg name="rvizconfig" default="$(find wpr_simulation)/rviz/nav.rviz" />
<param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<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" >
<rosparam command="load" file="$(find wpb_home_bringup)/config/wpb_home.yaml" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
<!-- clear_costmap -->
<!-- <node pkg="rover_bringup" type="clear_costmap" name="clear_costmap" /> -->
<!-- <node name="jie_localization" pkg="jie_ware" type="jie_localization" /> -->
</launch>

74
launch/lidar_loc_test.launch Normal file
View File

@ -0,0 +1,74 @@
<launch>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find wpr_simulation)/worlds/robocup_home.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="recording" value="false"/>
<arg name="debug" value="false"/>
</include>
<!-- Spawn the objects into Gazebo -->
<node name="bed" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bed.model -x 5.0 -y -3.9 -z 0 -Y 3.14159 -urdf -model bed" />
<node name="sofa" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/sofa.model -x -1.0 -y -3.9 -z 0 -Y 1.57 -urdf -model sofa" />
<node name="tea_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/tea_table.model -x -2.1 -y -2.2 -z 0 -Y 1.57 -urdf -model tea_table" />
<node name="bookshelft" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/bookshelft.model -x 2.0 -y -0.55 -z 0 -Y -1.57 -urdf -model bookshelft" />
<node name="kitchen_table" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x -3.5 -y 3.7 -z 0 -Y 1.57 -urdf -model kitchen_table" />
<node name="cupboard_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -2.0 -y 0.7 -z 0 -Y 1.57 -urdf -model cupboard_0" />
<node name="cupboard_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/cupboard.model -x -1.3 -y 3.7 -z 0 -Y -1.57 -urdf -model cupboard_1" />
<node name="dinning_table_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_0" />
<node name="dinning_table_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 1.5 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_1" />
<node name="dinning_table_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 1.5 -z 0 -Y 1.57 -urdf -model dinning_table_2" />
<node name="dinning_table_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/table.model -x 2.7 -y 2.0 -z 0 -Y 1.57 -urdf -model dinning_table_3" />
<node name="chair_0" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_0" />
<node name="chair_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 1.5 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_1" />
<node name="chair_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 1.2 -z 0 -Y 1.57 -urdf -model chair_2" />
<node name="chair_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_3" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home.model -urdf -x -0.0 -y -0.0 -model wpb_home" />
<!-- Run the map server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
<!-- Run lidar_loc -->
<node pkg="jie_ware" type="lidar_loc" name="lidar_loc" >
<param name="base_frame" value="base_footprint" />
<param name="laser_frame" value="laser" />
</node>
<!--- Run move base -->
<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_planner_params.yaml" command="load" />
<param name="base_global_planner" value="global_planner/GlobalPlanner" />
<param name="use_dijkstra" value="true"/>
<param name="base_local_planner" value="wpbh_local_planner/WpbhLocalPlanner" />
<param name= "controller_frequency" value="10" type="double"/>
</node>
<!-- RViz and TF tree -->
<arg name="model" default="$(find wpb_home_bringup)/urdf/wpb_home.urdf"/>
<arg name="gui" default="false" />
<arg name="rvizconfig" default="$(find wpr_simulation)/rviz/nav.rviz" />
<param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
<param name="use_gui" value="$(arg gui)"/>
<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" >
<rosparam command="load" file="$(find wpb_home_bringup)/config/wpb_home.yaml" />
</node>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

87
package.xml Normal file
View File

@ -0,0 +1,87 @@
<?xml version="1.0"?>
<package format="2">
<name>jie_ware</name>
<version>0.1.0</version>
<description>The jie_ware package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="zhangwanjie@6-robot.com">Zhang Wanjie</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/wpb_home</url> -->
<url type="website">http://www.6-robot.com</url>
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>move_base_msgs</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>tf</build_export_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>move_base_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

425
src/lidar_loc.cpp Normal file
View File

@ -0,0 +1,425 @@
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/RegionOfInterest.h>
#include <sensor_msgs/LaserScan.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
#include <vector>
#include <cmath>
nav_msgs::OccupancyGrid map_msg;
cv::Mat map_cropped;
cv::Mat map_temp;
cv::Mat map_match;
sensor_msgs::RegionOfInterest map_roi_info;
std::vector<cv::Point2f> scan_points;
std::vector<cv::Point2f> transform_points;
ros::ServiceClient clear_costmaps_client;
std::string base_frame;
std::string laser_frame;
float lidar_x = 250, lidar_y = 250, lidar_yaw = 0;
int cur_sum = 0;
int clear_countdown = -1;
// 初始姿态回调函数
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
try {
static tf2_ros::Buffer tfBuffer;
static tf2_ros::TransformListener tfListener(tfBuffer);
// 1. 查询从 base_frame 到 laser_frame 的转换
geometry_msgs::TransformStamped transformStamped =
tfBuffer.lookupTransform(base_frame, laser_frame, ros::Time(0), ros::Duration(1.0));
// 2. 创建一个 stamped pose 用于转换
geometry_msgs::PoseStamped base_pose, laser_pose;
base_pose.header = msg->header;
base_pose.pose = msg->pose.pose;
// 3. 将 base_footprint 的位姿转换为 laser 的位姿
tf2::doTransform(base_pose, laser_pose, transformStamped);
// 4. 从转换后的消息中提取位置和方向信息
double x = msg->pose.pose.position.x;
double y = msg->pose.pose.position.y;
tf2::Quaternion q(
laser_pose.pose.orientation.x,
laser_pose.pose.orientation.y,
laser_pose.pose.orientation.z,
laser_pose.pose.orientation.w);
// 5. 将四元数转换为yaw角度
tf2::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
// 6. 将地图坐标转换为OpenCV坐标（OpenCV的y轴是向下的，而ROS的y轴是向上的）
lidar_x = (x - map_msg.info.origin.position.x) / map_msg.info.resolution - map_roi_info.x_offset;
lidar_y = (y - map_msg.info.origin.position.y) / map_msg.info.resolution - map_roi_info.y_offset;
// 7. 设置yaw角度
lidar_yaw = -yaw;
clear_countdown = 30;
}
catch (tf2::TransformException &ex)
{
ROS_WARN("无法获取从 %s 到 %s 的转换: %s", base_frame.c_str(), laser_frame.c_str(), ex.what());
}
}
void crop_map();
void processMap();
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
map_msg = *msg;
crop_map();
}
void crop_map()
{
// 显示地图信息
std_msgs::Header header = map_msg.header;
nav_msgs::MapMetaData info = map_msg.info;
//用来统计地图有效区域的变量
int xMax,xMin,yMax,yMin ;
xMax=xMin= info.width/2;
yMax=yMin= info.height/2;
bool bFirstPoint = true;
// 把地图数据转换成图片
cv::Mat map_raw(info.height, info.width, CV_8UC1, cv::Scalar(128)); // 灰色背景
for(int y = 0; y < info.height; y++)
{
for(int x = 0; x < info.width; x++)
{
int index = y * info.width + x;
// 直接使用map_msg.data中的值
map_raw.at<uchar>(y, x) = static_cast<uchar>(map_msg.data[index]);
// 统计有效区域
if(map_msg.data[index] != -1)
{
if(bFirstPoint)
{
xMax = xMin = x;
yMax = yMin = y;
bFirstPoint = false;
continue;
}
xMin = std::min(xMin, x);
xMax = std::max(xMax, x);
yMin = std::min(yMin, y);
yMax = std::max(yMax, y);
}
}
}
// 计算有效区域的中心点坐标
int cen_x = (xMin + xMax)/2;
int cen_y = (yMin + yMax)/2;
// 按照有效区域对地图进行裁剪
int new_half_width = abs(xMax - xMin)/2 + 50;
int new_half_height = abs(yMax - yMin)/2 + 50;
int new_origin_x = cen_x - new_half_width;
int new_origin_y = cen_y - new_half_height;
int new_width = new_half_width*2;
int new_height = new_half_height*2;
cv::Rect roi(new_origin_x, new_origin_y, new_width, new_height);
cv::Mat roi_map = map_raw(roi).clone();
map_cropped = roi_map;
// 地图的裁减信息
map_roi_info.x_offset = new_origin_x;
map_roi_info.y_offset = new_origin_y;
map_roi_info.width = new_width;
map_roi_info.height = new_height;
geometry_msgs::PoseWithCovarianceStamped init_pose;
init_pose.pose.pose.position.x = 0.0;
init_pose.pose.pose.position.y = 0.0;
init_pose.pose.pose.position.y = 0.0;
init_pose.pose.pose.orientation.x = 0.0;
init_pose.pose.pose.orientation.y = 0.0;
init_pose.pose.pose.orientation.z = 0.0;
init_pose.pose.pose.orientation.w = 1.0;
geometry_msgs::PoseWithCovarianceStamped::ConstPtr init_pose_ptr(new geometry_msgs::PoseWithCovarianceStamped(init_pose));
initialPoseCallback(init_pose_ptr);
}
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
std::vector<cv::Point2f> scan_recv;
// scan_points.clear();
double angle = msg->angle_min;
for (size_t i = 0; i < msg->ranges.size(); ++i)
{
if (msg->ranges[i] >= msg->range_min && msg->ranges[i] <= msg->range_max)
{
float x = msg->ranges[i] * cos(angle) / map_msg.info.resolution;
float y = -msg->ranges[i] * sin(angle) / map_msg.info.resolution;
scan_recv.push_back(cv::Point2f(x, y));
}
angle += msg->angle_increment;
}
scan_points = scan_recv;
if(clear_countdown > -1)
clear_countdown --;
if(clear_countdown == 0)
{
std_srvs::Empty srv;
clear_costmaps_client.call(srv);
}
}
cv::Mat createGradientMask(int size)
{
cv::Mat mask(size, size, CV_8UC1);
int center = size / 2;
for (int y = 0; y < size; y++)
{
for (int x = 0; x < size; x++)
{
double distance = std::hypot(x - center, y - center);
int value = cv::saturate_cast<uchar>(255 * std::max(0.0, 1.0 - distance / center));
mask.at<uchar>(y, x) = value;
}
}
return mask;
}
void processMap()
{
if (map_cropped.empty()) return;
map_temp = cv::Mat::zeros(map_cropped.size(), CV_8UC1);
cv::Mat gradient_mask = createGradientMask(101); // 创建一个101x101的渐变掩模
for (int y = 0; y < map_cropped.rows; y++)
{
for (int x = 0; x < map_cropped.cols; x++)
{
if (map_cropped.at<uchar>(y, x) == 100) // 障碍物
{
int left = std::max(0, x - 50);
int top = std::max(0, y - 50);
int right = std::min(map_cropped.cols - 1, x + 50);
int bottom = std::min(map_cropped.rows - 1, y + 50);
cv::Rect roi(left, top, right - left + 1, bottom - top + 1);
cv::Mat region = map_temp(roi);
int mask_left = 50 - (x - left);
int mask_top = 50 - (y - top);
cv::Rect mask_roi(mask_left, mask_top, roi.width, roi.height);
cv::Mat mask = gradient_mask(mask_roi);
cv::max(region, mask, region);
}
}
}
}
void pose_tf()
{
if (map_cropped.empty() || map_msg.data.empty()) return;
static tf2_ros::Buffer tfBuffer;
static tf2_ros::TransformListener tfListener(tfBuffer);
// 1. 计算在裁剪地图中的实际米制坐标
double x_meters = (lidar_x + map_roi_info.x_offset) * map_msg.info.resolution;
double y_meters = (map_cropped.rows - lidar_y + map_roi_info.y_offset) * map_msg.info.resolution;
// 2. 考虑原始地图的原点偏移
x_meters += map_msg.info.origin.position.x;
y_meters += map_msg.info.origin.position.y;
// 3. 处理yaw角度
double yaw_ros = -lidar_yaw;
// 4. 将弧度转换为四元数
tf2::Quaternion q;
q.setRPY(0, 0, yaw_ros);
// 5. 计算 base_footprint 在 map 中的位置
double base_x = x_meters;
double base_y = -y_meters;
// 6. 查询 odom 到 base_frame 的变换
geometry_msgs::TransformStamped odom_to_base;
try {
odom_to_base = tfBuffer.lookupTransform("odom", laser_frame, ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
return;
}
// 7. 计算 map 到 odom 的变换
tf2::Transform map_to_base, odom_to_base_tf2;
map_to_base.setOrigin(tf2::Vector3(base_x, base_y, 0));
map_to_base.setRotation(q);
tf2::fromMsg(odom_to_base.transform, odom_to_base_tf2);
tf2::Transform map_to_odom = map_to_base * odom_to_base_tf2.inverse();
// 8. 发布 map 到 odom 的变换
static tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped map_to_odom_msg;
map_to_odom_msg.header.stamp = ros::Time::now();
map_to_odom_msg.header.frame_id = "map";
map_to_odom_msg.child_frame_id = "odom";
map_to_odom_msg.transform = tf2::toMsg(map_to_odom);
// 计算 yaw 角度
tf2::Quaternion q_tf2(
map_to_odom_msg.transform.rotation.x,
map_to_odom_msg.transform.rotation.y,
map_to_odom_msg.transform.rotation.z,
map_to_odom_msg.transform.rotation.w);
tf2::Matrix3x3 m(q_tf2);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
br.sendTransform(map_to_odom_msg);
}
int main(int argc, char** argv)
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "lidar_loc");
// 读取参数
ros::NodeHandle private_nh("~");
private_nh.param<std::string>("base_frame", base_frame, "base_footprint");
private_nh.param<std::string>("laser_frame", laser_frame, "laser");
ros::NodeHandle nh;
ros::Subscriber map_sub = nh.subscribe("/map", 1, mapCallback);
ros::Subscriber scan_sub = nh.subscribe("/scan", 1, scanCallback);
ros::Subscriber initial_pose_sub = nh.subscribe("/initialpose", 1, initialPoseCallback);
clear_costmaps_client = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
ros::Rate rate(50); // 匹配频率
while (ros::ok())
{
if (!map_cropped.empty())
{
processMap();
map_match = map_temp.clone();
cv::cvtColor(map_match, map_match, cv::COLOR_GRAY2BGR);
// 遍历 map_cropped，将 map_match 中对应的障碍物像素设置为紫色
for (int y = 0; y < map_cropped.rows; ++y)
{
for (int x = 0; x < map_cropped.cols; ++x)
{
if (map_cropped.at<uchar>(y, x) == 100) // 100 表示障碍物
{
map_match.at<cv::Vec3b>(y, x) = cv::Vec3b(128, 0, 128); // 紫色 (BGR格式)
}
}
}
// 计算三种情况下的雷达点坐标数组
std::vector<cv::Point2f> transform_points, clockwise_points, counter_points;
float deg_to_rad = M_PI / 180.0;
int max_sum = 0;
float best_dx = 0, best_dy = 0, best_dyaw = 0;
for (const auto& point : scan_points)
{
// 情况一：原始角度
float rotated_x = point.x * cos(lidar_yaw) - point.y * sin(lidar_yaw);
float rotated_y = point.x * sin(lidar_yaw) + point.y * cos(lidar_yaw);
transform_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y));
// 情况二：顺时针旋转1度
float clockwise_yaw = lidar_yaw + deg_to_rad;
rotated_x = point.x * cos(clockwise_yaw) - point.y * sin(clockwise_yaw);
rotated_y = point.x * sin(clockwise_yaw) + point.y * cos(clockwise_yaw);
clockwise_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y));
// 情况三：逆时针旋转1度
float counter_yaw = lidar_yaw - deg_to_rad;
rotated_x = point.x * cos(counter_yaw) - point.y * sin(counter_yaw);
rotated_y = point.x * sin(counter_yaw) + point.y * cos(counter_yaw);
counter_points.push_back(cv::Point2f(rotated_x + lidar_x, lidar_y - rotated_y));
}
// 计算15种变换方式的匹配值
std::vector<cv::Point2f> offsets = {{0,0}, {1,0}, {-1,0}, {0,1}, {0,-1}};
std::vector<std::vector<cv::Point2f>> point_sets = {transform_points, clockwise_points, counter_points};
std::vector<float> yaw_offsets = {0, deg_to_rad, -deg_to_rad};
for (int i = 0; i < offsets.size(); ++i)
{
for (int j = 0; j < point_sets.size(); ++j)
{
int sum = 0;
for (const auto& point : point_sets[j])
{
float px = point.x + offsets[i].x;
float py = point.y + offsets[i].y;
if (px >= 0 && px < map_match.cols && py >= 0 && py < map_match.rows)
{
sum += map_temp.at<uchar>(py, px);
}
}
if (sum > max_sum)
{
max_sum = sum;
best_dx = offsets[i].x;
best_dy = offsets[i].y;
best_dyaw = yaw_offsets[j];
}
}
}
// 更新雷达位置和角度
lidar_x += best_dx;
lidar_y += best_dy;
lidar_yaw += best_dyaw;
// 绘制最佳匹配的测距点
for (const auto& point : transform_points)
{
float px = point.x + best_dx;
float py = point.y + best_dy;
if (px >= 0 && px < map_match.cols && py >= 0 && py < map_match.rows)
{
map_match.at<cv::Vec3b>(py, px) = cv::Vec3b(0, 0, 255); // BGR 格式：红色
}
}
}
pose_tf();
ros::spinOnce();
rate.sleep();
}
return 0;
}