robofish/rc_navigation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加了点云转2d

Browse Source
This commit is contained in:
Robofish 2024-10-06 14:55:15 +08:00
parent baec84c783
commit 13e648b894
23 changed files with 1622 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -50,3 +50,5 @@ qtcreator-*
# Catkin custom files
CATKIN_IGNORE
.catkin_workspace

2
ptl.sh Normal file
View File

@ -0,0 +1,2 @@
source devel/setup.bash
roslaunch pointcloud_to_laserscan point_to_scan.launch

72
src/pointcloud_to_laserscan/CHANGELOG.rst Normal file
View File

@ -0,0 +1,72 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package pointcloud_to_laserscan
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.4.0 (2017-11-14)
------------------
* Added inf_epsilon parameter that determines the value added to max range when use_infs parameter is set to false
* Merge pull request `#11 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/11>`_ from ros-perception/mikaelarguedas-patch-1
update to use non deprecated pluginlib macro
* Migrate to package format 2; add roslint
* Add sane parameter defaults; fixup lint warnings
* update to use non deprecated pluginlib macro
* Contributors: Mikael Arguedas, Paul Bovbel, Prasanna Kannappan
1.3.1 (2017-04-26)
------------------
* Merge pull request `#4 <https://github.com/ros-perception/pointcloud_to_laserscan/issues/4>`_ from yoshimalucky/fix-miscalculation-in-angle-increment
Fixed miscalculation in angle_increment in the launch files.
* fixed miscalculation in angle_increment in the launchfiles.
* Contributors: Paul Bovbel, yoshimalucky
1.3.0 (2015-06-09)
------------------
* Fix pointcloud to laserscan transform tolerance issues
* Move pointcloud_to_laserscan to new repository
* Contributors: Paul Bovbel
1.2.7 (2015-06-08)
------------------
* Cleanup pointcloud_to_laserscan launch files
* Contributors: Paul Bovbel
1.2.6 (2015-02-04)
------------------
* Fix default value for concurrency
* Fix multithreaded lazy pub sub
* Contributors: Paul Bovbel
1.2.5 (2015-01-20)
------------------
* Switch to tf_sensor_msgs for transform
* Set parameters in sample launch files to default
* Add tolerance parameter
* Contributors: Paul Bovbel
1.2.4 (2015-01-15)
------------------
* Remove stray dependencies
* Refactor with tf2 and message filters
* Remove roslaunch check
* Fix regressions
* Refactor to allow debug messages from node and nodelet
* Contributors: Paul Bovbel
1.2.3 (2015-01-10)
------------------
* add launch tests
* refactor naming and fix nodelet export
* set default target frame to empty
* clean up package.xml
* Contributors: Paul Bovbel
1.2.2 (2014-10-25)
------------------
* clean up package.xml
* Fix header reference
* Fix flow
* Fix pointer assertion
* Finalize pointcloud to laserscan
* Initial pointcloud to laserscan commit
* Contributors: Paul Bovbel

51
src/pointcloud_to_laserscan/CMakeLists.txt Normal file
View File

@ -0,0 +1,51 @@
cmake_minimum_required(VERSION 2.8.3)
project(pointcloud_to_laserscan)
find_package(catkin REQUIRED COMPONENTS
message_filters
nodelet
roscpp
sensor_msgs
tf2
tf2_ros
tf2_sensor_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES pointcloud_to_laserscan
CATKIN_DEPENDS roscpp message_filters nodelet sensor_msgs tf2 tf2_ros
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(pointcloud_to_laserscan src/pointcloud_to_laserscan_nodelet.cpp)
target_link_libraries(pointcloud_to_laserscan ${catkin_LIBRARIES})
add_executable(pointcloud_to_laserscan_node src/pointcloud_to_laserscan_node.cpp)
target_link_libraries(pointcloud_to_laserscan_node pointcloud_to_laserscan ${catkin_LIBRARIES})
install(TARGETS pointcloud_to_laserscan pointcloud_to_laserscan_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(FILES nodelets.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
if(CATKIN_ENABLE_TESTING)
find_package(roslint REQUIRED)
roslint_cpp()
roslint_add_test()
endif()

2
src/pointcloud_to_laserscan/README.md Normal file
View File

@ -0,0 +1,2 @@
# pointcloud_to_laserscan
ROS1 Package for pointcloud_to_laserscan. Works well with ROS-Noetic

99
src/pointcloud_to_laserscan/include/pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h Normal file
View File

@ -0,0 +1,99 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010-2012, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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,
* INCIDENTAL, 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: Paul Bovbel
*/
#ifndef POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
#define POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET
#include "ros/ros.h"
#include "boost/thread/mutex.hpp"
#include "nodelet/nodelet.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "sensor_msgs/PointCloud2.h"
namespace pointcloud_to_laserscan
{
typedef tf2_ros::MessageFilter<sensor_msgs::PointCloud2> MessageFilter;
/**
* Class to process incoming pointclouds into laserscans. Some initial code was pulled from the defunct turtlebot
* pointcloud_to_laserscan implementation.
*/
class PointCloudToLaserScanNodelet : public nodelet::Nodelet
{
public:
PointCloudToLaserScanNodelet();
private:
virtual void onInit();
void cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg);
void failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
tf2_ros::filter_failure_reasons::FilterFailureReason reason);
void connectCb();
void disconnectCb();
ros::NodeHandle nh_, private_nh_;
ros::Publisher pub_;
boost::mutex connect_mutex_;
boost::shared_ptr<tf2_ros::Buffer> tf2_;
boost::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
message_filters::Subscriber<sensor_msgs::PointCloud2> sub_;
boost::shared_ptr<MessageFilter> message_filter_;
// ROS Parameters
unsigned int input_queue_size_;
std::string target_frame_;
double tolerance_;
double min_height_, max_height_, angle_min_, angle_max_, angle_increment_, scan_time_, range_min_, range_max_;
bool use_inf_;
double inf_epsilon_;
};
} // pointcloud_to_laserscan
#endif // POINTCLOUD_TO_LASERSCAN_POINTCLOUD_TO_LASERSCAN_NODELET

52
src/pointcloud_to_laserscan/launch/point_to_scan.launch Normal file
View File

@ -0,0 +1,52 @@
<?xml version="1.0"?>
<launch>
<!-- run pointcloud_to_laserscan node -->
<!--node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="/rslidar_points"/>
<rosparam>
# target_frame: rslidar # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.01
min_height: -0.4
max_height: 1.0
angle_min: -3.1415926 # -M_PI
angle_max: 3.1415926 # M_PI
angle_increment: 0.003 # 0.17degree
scan_time: 0.1
range_min: 0.2
range_max: 100
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
</node-->
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node"
name="pointcloud_to_laserscan">
<remap from="cloud_in" to="/cloud_registered_body"/>
<remap from="scan" to="/scan"/>
<param name="transform_tolerance" value="0.01" />
<param name="min_height" value="-0.4" />
<param name="max_height" value="1" />
<param name="angle_min" value="-3.1425926" />
<param name="angle_max" value="3.1415926" />
<param name="angle_increment" value="0.007" />
<param name="scan_time" value="0.1" />
<param name="range_min" value="0.2" />
<param name="range_max" value="100" />
<param name="use_inf" value="true" />
<param name="inf_epsilon" value="1.0" />
<param name="concurrency_level" value="1" />
</node>
</launch>

41
src/pointcloud_to_laserscan/launch/sample_node.launch Normal file
View File

@ -0,0 +1,41 @@
<?xml version="1.0"?>
<launch>
<arg name="camera" default="camera" />
<!-- start sensor-->
<include file="$(find openni2_launch)/launch/openni2.launch">
<arg name="camera" default="$(arg camera)"/>
</include>
<!-- run pointcloud_to_laserscan node -->
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
<remap from="scan" to="$(arg camera)/scan"/>
<rosparam>
target_frame: camera_link # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.01
min_height: 0.0
max_height: 1.0
angle_min: -1.5708 # -M_PI/2
angle_max: 1.5708 # M_PI/2
angle_increment: 0.0087 # M_PI/360.0
scan_time: 0.3333
range_min: 0.45
range_max: 4.0
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
</node>
</launch>

41
src/pointcloud_to_laserscan/launch/sample_nodelet.launch Normal file
View File

@ -0,0 +1,41 @@
<?xml version="1.0"?>
<launch>
<arg name="camera" default="camera" />
<!-- start sensor-->
<include file="$(find openni2_launch)/launch/openni2.launch">
<arg name="camera" default="$(arg camera)"/>
</include>
<!-- push pointcloud_to_laserscan nodelet into sensor's nodelet manager-->
<node pkg="nodelet" type="nodelet" name="pointcloud_to_laserscan" args="load pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet $(arg camera)_nodelet_manager">
<remap from="cloud_in" to="$(arg camera)/depth_registered/points_processed"/>
<remap from="scan" to="$(arg camera)/scan"/>
<rosparam>
target_frame: camera_link # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.01
min_height: 0.0
max_height: 1.0
angle_min: -1.5708 # -M_PI/2
angle_max: 1.5708 # M_PI/2
angle_increment: 0.0087 # M_PI/360.0
scan_time: 0.3333
range_min: 0.45
range_max: 4.0
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing, thread number governed by nodelet manager
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
</node>
</launch>

36
src/pointcloud_to_laserscan/launch/xiaoqiang_lungu_kinect.launch Normal file
View File

@ -0,0 +1,36 @@
<?xml version="1.0"?>
<launch>
<!-- run pointcloud_to_laserscan node -->
<node pkg="tf" type="static_transform_publisher" name="baselink2kinect_scan"
args="0.1 0 0.3 0 0 0 base_link kinect_link 100" />
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="/kinect/depth/points"/>
<remap from="scan" to="/kinect/scan"/>
<rosparam>
target_frame: kinect_link # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.1
min_height: -0.2
max_height: 1.0
angle_min: -0.79 # -M_PI
angle_max: 0.79 # M_PI
angle_increment: 0.003 # 0.17degree
scan_time: 0.05
range_min: 0.45
range_max: 5.0
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
</node>
</launch>

36
src/pointcloud_to_laserscan/launch/xiaoqiang_pro_kinect.launch Normal file
View File

@ -0,0 +1,36 @@
<?xml version="1.0"?>
<launch>
<!-- run pointcloud_to_laserscan node -->
<node pkg="tf" type="static_transform_publisher" name="baselink2kinect_scan"
args="-0.15 0 0.3 0 0 0 base_link kinect_link 100" />
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="/kinect/depth/points"/>
<remap from="scan" to="/kinect/scan"/>
<rosparam>
target_frame: kinect_link # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.1
min_height: -0.2
max_height: 1.0
angle_min: -0.79 # -M_PI
angle_max: 0.79 # M_PI
angle_increment: 0.003 # 0.17degree
scan_time: 0.05
range_min: 0.45
range_max: 5.0
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
</node>
</launch>

34
src/pointcloud_to_laserscan/launch/xiaoqiang_rslidar.launch Normal file
View File

@ -0,0 +1,34 @@
<?xml version="1.0"?>
<launch>
<!-- run pointcloud_to_laserscan node -->
<node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
<remap from="cloud_in" to="/rslidar_points"/>
<remap from="scan" to="/rslidar/scan"/>
<rosparam>
# target_frame: rslidar # Leave disabled to output scan in pointcloud frame
transform_tolerance: 0.01
min_height: -0.4
max_height: 1.0
angle_min: -3.1415926 # -M_PI
angle_max: 3.1415926 # M_PI
angle_increment: 0.003 # 0.17degree
scan_time: 0.1
range_min: 0.2
range_max: 100
use_inf: true
inf_epsilon: 1.0
# Concurrency level, affects number of pointclouds queued for processing and number of threads used
# 0 : Detect number of cores
# 1 : Single threaded
# 2->inf : Parallelism level
concurrency_level: 1
</rosparam>
</node>
</launch>

11
src/pointcloud_to_laserscan/nodelets.xml Normal file
View File

@ -0,0 +1,11 @@
<library path="lib/libpointcloud_to_laserscan">
<class name="pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet"
type="pointcloud_to_laserscan::PointCloudToLaserScanNodelet"
base_class_type="nodelet::Nodelet">
<description>
Nodelet to convert sensor_msgs/PointCloud2 to sensor_msgs/LaserScans.
</description>
</class>
</library>

32
src/pointcloud_to_laserscan/package.xml Normal file
View File

@ -0,0 +1,32 @@
<?xml version="1.0"?>
<package format="2">
<name>pointcloud_to_laserscan</name>
<version>1.4.0</version>
<description>Converts a 3D Point Cloud into a 2D laser scan. This is useful for making devices like the Kinect appear like a laser scanner for 2D-based algorithms (e.g. laser-based SLAM).</description>
<maintainer email="paul@bovbel.com">Paul Bovbel</maintainer>
<author email="paul@bovbel.com">Paul Bovbel</author>
<author>Tully Foote</author>
<license>BSD</license>
<url type="website">http://ros.org/wiki/perception_pcl</url>
<url type="bugtracker">https://github.com/ros-perception/perception_pcl/issues</url>
<url type="repository">https://github.com/ros-perception/perception_pcl</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>message_filters</depend>
<depend>nodelet</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_sensor_msgs</depend>
<test_depend>roslint</test_depend>
<export>
<nodelet plugin="${prefix}/nodelets.xml"/>
</export>
</package>

229
src/pointcloud_to_laserscan/rviz/kinect.rviz Normal file
View File

@ -0,0 +1,229 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1
- /PointCloud21
- /LaserScan1
Splitter Ratio: 0.689373314
Tree Height: 722
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 200
Reference Frame: kinect_link
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 2.6387887
Min Value: -0.46631065
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Flat Squares
Topic: /kinect/depth/points
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Flat Squares
Topic: /kinect/scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front1_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front2_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
kinect_Link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
lidar_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: kinect_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 4.48610306
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 1.73725533
Y: -0.717763484
Z: -0.839548349
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.46479708
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.29040217
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1032
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000026e0000036dfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002a0000036d000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002b5000000fa0000000000000000000000010000010f0000036dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002a0000036d000000ba00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074f00000047fc0100000002fb0000000800540069006d006501000000000000074f0000031800fffffffb0000000800540069006d00650100000000000004500000000000000000000004db0000036d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1871
X: 49
Y: 24

233
src/pointcloud_to_laserscan/rviz/rsldiar.rviz Normal file
View File

@ -0,0 +1,233 @@
Panels:
- Class: rviz/Displays
Help Height: 89
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 735
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 0.5
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 20
Reference Frame: base_footprint
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 1.67712891
Min Value: -1.40163112
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 0
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 255
Selectable: true
Size (Pixels): 2
Size (m): 0.00999999978
Style: Points
Topic: /rslidar_points
Unreliable: true
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Axes
Enabled: false
Length: 1
Name: Axes
Radius: 0.100000001
Reference Frame: <Fixed Frame>
Value: false
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front1_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front2_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
kinect_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
lidar_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Flat Squares
Topic: /rslidar/scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Enabled: true
Global Options:
Background Color: 121; 129; 128
Default Light: true
Fixed Frame: rslidar
Frame Rate: 10
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/ThirdPersonFollower
Distance: 8.04747581
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.50192821
Y: 0.66879797
Z: -2.41525704e-05
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.56979632
Target Frame: aft_mapped
Value: ThirdPersonFollower (rviz)
Yaw: 2.59856272
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000016f00000385fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002a00000385000000d900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000394fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000394000000ba00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074f00000047fc0100000002fb0000000800540069006d006501000000000000074f0000031800fffffffb0000000800540069006d00650100000000000004500000000000000000000005da0000038500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1871
X: 49
Y: 24

65
src/pointcloud_to_laserscan/src/pointcloud_to_laserscan_node.cpp Normal file
View File

@ -0,0 +1,65 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010-2012, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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,
* INCIDENTAL, 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: Paul Bovbel
*/
#include <ros/ros.h>
#include <nodelet/loader.h>
int main(int argc, char **argv){
ros::init(argc, argv, "pointcloud_to_laserscan_node");
ros::NodeHandle private_nh("~");
int concurrency_level;
private_nh.param<int>("concurrency_level", concurrency_level, 0);
nodelet::Loader nodelet;
nodelet::M_string remap(ros::names::getRemappings());
nodelet::V_string nargv;
std::string nodelet_name = ros::this_node::getName();
nodelet.load(nodelet_name, "pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet", remap, nargv);
boost::shared_ptr<ros::MultiThreadedSpinner> spinner;
if(concurrency_level) {
spinner.reset(new ros::MultiThreadedSpinner(concurrency_level));
}else{
spinner.reset(new ros::MultiThreadedSpinner());
}
spinner->spin();
return 0;
}

248
src/pointcloud_to_laserscan/src/pointcloud_to_laserscan_nodelet.cpp Normal file
View File

@ -0,0 +1,248 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010-2012, Willow Garage, Inc.
* 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 Willow Garage, Inc. 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,
* INCIDENTAL, 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: Paul Bovbel
*/
#include <pointcloud_to_laserscan/pointcloud_to_laserscan_nodelet.h>
#include <sensor_msgs/LaserScan.h>
#include <pluginlib/class_list_macros.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
namespace pointcloud_to_laserscan
{
PointCloudToLaserScanNodelet::PointCloudToLaserScanNodelet() {}
void PointCloudToLaserScanNodelet::onInit()
{
boost::mutex::scoped_lock lock(connect_mutex_);
private_nh_ = getPrivateNodeHandle();
private_nh_.param<std::string>("target_frame", target_frame_, "");
private_nh_.param<double>("transform_tolerance", tolerance_, 0.01);
private_nh_.param<double>("min_height", min_height_, std::numeric_limits<double>::min());
private_nh_.param<double>("max_height", max_height_, std::numeric_limits<double>::max());
private_nh_.param<double>("angle_min", angle_min_, - M_PI);
private_nh_.param<double>("angle_max", angle_max_, M_PI);
private_nh_.param<double>("angle_increment", angle_increment_, M_PI / 180.0);
private_nh_.param<double>("scan_time", scan_time_, 1.0 / 30.0);
private_nh_.param<double>("range_min", range_min_, 0.0);
private_nh_.param<double>("range_max", range_max_, std::numeric_limits<double>::max());
private_nh_.param<double>("inf_epsilon", inf_epsilon_, 1.0);
int concurrency_level;
private_nh_.param<int>("concurrency_level", concurrency_level, 1);
private_nh_.param<bool>("use_inf", use_inf_, true);
//Check if explicitly single threaded, otherwise, let nodelet manager dictate thread pool size
if (concurrency_level == 1)
{
nh_ = getNodeHandle();
}
else
{
nh_ = getMTNodeHandle();
}
// Only queue one pointcloud per running thread
if (concurrency_level > 0)
{
input_queue_size_ = concurrency_level;
}
else
{
input_queue_size_ = boost::thread::hardware_concurrency();
}
// if pointcloud target frame specified, we need to filter by transform availability
if (!target_frame_.empty())
{
tf2_.reset(new tf2_ros::Buffer());
tf2_listener_.reset(new tf2_ros::TransformListener(*tf2_));
message_filter_.reset(new MessageFilter(sub_, *tf2_, target_frame_, input_queue_size_, nh_));
message_filter_->registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
message_filter_->registerFailureCallback(boost::bind(&PointCloudToLaserScanNodelet::failureCb, this, _1, _2));
}
else // otherwise setup direct subscription
{
sub_.registerCallback(boost::bind(&PointCloudToLaserScanNodelet::cloudCb, this, _1));
}
pub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 10,
boost::bind(&PointCloudToLaserScanNodelet::connectCb, this),
boost::bind(&PointCloudToLaserScanNodelet::disconnectCb, this));
}
void PointCloudToLaserScanNodelet::connectCb()
{
boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() > 0 && sub_.getSubscriber().getNumPublishers() == 0)
{
NODELET_INFO("Got a subscriber to scan, starting subscriber to pointcloud");
sub_.subscribe(nh_, "cloud_in", input_queue_size_);
}
}
void PointCloudToLaserScanNodelet::disconnectCb()
{
boost::mutex::scoped_lock lock(connect_mutex_);
if (pub_.getNumSubscribers() == 0)
{
NODELET_INFO("No subscibers to scan, shutting down subscriber to pointcloud");
sub_.unsubscribe();
}
}
void PointCloudToLaserScanNodelet::failureCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg,
tf2_ros::filter_failure_reasons::FilterFailureReason reason)
{
NODELET_WARN_STREAM_THROTTLE(1.0, "Can't transform pointcloud from frame " << cloud_msg->header.frame_id << " to "
<< message_filter_->getTargetFramesString() << " at time " << cloud_msg->header.stamp << ", reason: " << reason);
}
void PointCloudToLaserScanNodelet::cloudCb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{
//build laserscan output
sensor_msgs::LaserScan output;
output.header = cloud_msg->header;
if (!target_frame_.empty())
{
output.header.frame_id = target_frame_;
}
output.angle_min = angle_min_;
output.angle_max = angle_max_;
output.angle_increment = angle_increment_;
output.time_increment = 0.0;
output.scan_time = scan_time_;
output.range_min = range_min_;
output.range_max = range_max_;
//determine amount of rays to create
uint32_t ranges_size = std::ceil((output.angle_max - output.angle_min) / output.angle_increment);
//determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
if (use_inf_)
{
output.ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
}
else
{
output.ranges.assign(ranges_size, output.range_max + inf_epsilon_);
}
sensor_msgs::PointCloud2ConstPtr cloud_out;
sensor_msgs::PointCloud2Ptr cloud;
// Transform cloud if necessary
if (!(output.header.frame_id == cloud_msg->header.frame_id))
{
try
{
cloud.reset(new sensor_msgs::PointCloud2);
tf2_->transform(*cloud_msg, *cloud, target_frame_, ros::Duration(tolerance_));
cloud_out = cloud;
}
catch (tf2::TransformException &ex)
{
NODELET_ERROR_STREAM("Transform failure: " << ex.what());
return;
}
}
else
{
cloud_out = cloud_msg;
}
// Iterate through pointcloud
for (sensor_msgs::PointCloud2ConstIterator<float>
iter_x(*cloud_out, "x"), iter_y(*cloud_out, "y"), iter_z(*cloud_out, "z");
iter_x != iter_x.end();
++iter_x, ++iter_y, ++iter_z)
{
if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z))
{
NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", *iter_x, *iter_y, *iter_z);
continue;
}
if (*iter_z > max_height_ || *iter_z < min_height_)
{
NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", *iter_z, min_height_, max_height_);
continue;
}
double range = hypot(*iter_x, *iter_y);
if (range < range_min_)
{
NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range, range_min_, *iter_x, *iter_y,
*iter_z);
continue;
}
if (range > range_max_)
{
NODELET_DEBUG("rejected for range %f above maximum value %f. Point: (%f, %f, %f)", range, range_max_, *iter_x, *iter_y,
*iter_z);
continue;
}
double angle = atan2(*iter_y, *iter_x);
if (angle < output.angle_min || angle > output.angle_max)
{
NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output.angle_min, output.angle_max);
continue;
}
//overwrite range at laserscan ray if new range is smaller
int index = (angle - output.angle_min) / output.angle_increment;
if (range < output.ranges[index])
{
output.ranges[index] = range;
}
}
pub_.publish(output);
}
}
PLUGINLIB_EXPORT_CLASS(pointcloud_to_laserscan::PointCloudToLaserScanNodelet, nodelet::Nodelet)

10
src/rc_driver/livox_ros_driver2/config/MID360_config.json
View File

@ -11,13 +11,13 @@
"log_data_port": 56500
},
"host_net_info" : {
"cmd_data_ip" : "192.168.1.5",
"cmd_data_ip" : "192.168.1.50",
"cmd_data_port": 56101,
"push_msg_ip": "192.168.1.5",
"push_msg_ip": "192.168.1.50",
"push_msg_port": 56201,
"point_data_ip": "192.168.1.5",
"point_data_ip": "192.168.1.50",
"point_data_port": 56301,
"imu_data_ip" : "192.168.1.5",
"imu_data_ip" : "192.168.1.50",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501
@ -25,7 +25,7 @@
},
"lidar_configs" : [
{
"ip" : "192.168.1.12",
"ip" : "192.168.1.176",
"pcl_data_type" : 1,
"pattern_mode" : 0,
"extrinsic_parameter" : {

1
src/rc_localization/FAST_LIO_LOCALIZATION-ROS-NOETIC Submodule

@ -0,0 +1 @@
Subproject commit 7fae5d1f9c0d9f6a64c6ac936f2239b9ee9cb18d

207
src/rc_navigation/CMakeLists.txt Normal file
View File

@ -0,0 +1,207 @@
cmake_minimum_required(VERSION 3.0.2)
project(rc_navigation)
## 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
amcl
gmapping
map_server
move_base
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## 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
# std_msgs # Or other packages containing 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 rc_navigation
# CATKIN_DEPENDS amcl gmapping map_server move_base
# 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}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/rc_navigation.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/rc_navigation_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_rc_navigation.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)

51
src/rc_navigation/launch/t1_slam.launch Normal file
View File

@ -0,0 +1,51 @@
<launch>
<!-- 设置为true表示当前环境是仿真环境 -->
<param name="use_sim_time" value="false" />
<!-- gamping 节点 -->
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<!-- 设置雷达话题 -->
<remap from="scan" to="scan" />
<!-- 关键参数：坐标系 -->
<param name="base_frame" value="base_footprint" /> <!--底盘坐标系-->
<param name="odom_frame" value="odom" /> <!--里程计坐标系-->
<param name="map_frame" value="map" /> <!--地图坐标系-->
<param name="map_update_interval" value="5.0" />
<param name="maxUrange" value="16.0" />
<param name="sigma" value="0.05" />
<param name="kernelSize" value="1" />
<param name="lstep" value="0.05" />
<param name="astep" value="0.05" />
<param name="iterations" value="5" />
<param name="lsigma" value="0.075" />
<param name="ogain" value="3.0" />
<param name="lskip" value="0" />
<param name="srr" value="0.1" />
<param name="srt" value="0.2" />
<param name="str" value="0.1" />
<param name="stt" value="0.2" />
<param name="linearUpdate" value="1.0" />
<param name="angularUpdate" value="0.5" />
<param name="temporalUpdate" value="3.0" />
<param name="resampleThreshold" value="0.5" />
<param name="particles" value="30" />
<param name="xmin" value="-50.0" />
<param name="ymin" value="-50.0" />
<param name="xmax" value="50.0" />
<param name="ymax" value="50.0" />
<param name="delta" value="0.05" />
<param name="llsamplerange" value="0.01" />
<param name="llsamplestep" value="0.01" />
<param name="lasamplerange" value="0.005" />
<param name="lasamplestep" value="0.005" />
</node>
<node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
<node pkg="rviz" type="rviz" name="rviz" />
<!-- 使用之前保存过的 rviz 配置-->
<!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf_gazebo)/config/t7_car.rviz" /> -->
</launch>

71
src/rc_navigation/package.xml Normal file
View File

@ -0,0 +1,71 @@
<?xml version="1.0"?>
<package format="2">
<name>rc_navigation</name>
<version>0.0.0</version>
<description>The rc_navigation package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robofish@todo.todo">robofish</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>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/rc_navigation</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>amcl</build_depend>
<build_depend>gmapping</build_depend>
<build_depend>map_server</build_depend>
<build_depend>move_base</build_depend>
<build_export_depend>amcl</build_export_depend>
<build_export_depend>gmapping</build_export_depend>
<build_export_depend>map_server</build_export_depend>
<build_export_depend>move_base</build_export_depend>
<exec_depend>amcl</exec_depend>
<exec_depend>gmapping</exec_depend>
<exec_depend>map_server</exec_depend>
<exec_depend>move_base</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>