mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-09-12 22:30:19 +08:00
ros-pkg: moved az3 planner config in this project
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1919 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -121,7 +121,6 @@ add_library(rtabmap_ros
|
||||
src/nodelets/data_throttle.cpp
|
||||
src/nodelets/stereo_throttle.cpp
|
||||
src/nodelets/data_odom_sync.cpp
|
||||
src/nodelets/disparity_odom_sync.cpp
|
||||
src/nodelets/point_cloud_xyzrgb.cpp
|
||||
src/nodelets/point_cloud_xyz.cpp
|
||||
src/nodelets/disparity_to_depth.cpp
|
||||
|
||||
@@ -11,30 +11,27 @@
|
||||
<include file="$(find az3_bringup)/az3_standalone.launch"/>
|
||||
|
||||
<node name="joy" pkg="joy" type="joy_node"/>
|
||||
|
||||
<group ns="teleop">
|
||||
<remap from="joy" to="/joy"/>
|
||||
<node name="teleop" pkg="nodelet" type="nodelet"
|
||||
args="standalone azimut_tools/Teleop"/>
|
||||
<node name="teleop" pkg="nodelet" type="nodelet" args="standalone azimut_tools/Teleop"/>
|
||||
<param name="cmd_eta/abtr_priority" value="50"/>
|
||||
</group>
|
||||
|
||||
<group ns="go_back">
|
||||
<remap from="joy" to="/joy"/>
|
||||
<remap from="goal" to="/planner_goal"/>
|
||||
<node name="go_back" pkg="azimut_tools" type="go_back"/>
|
||||
<rosparam>
|
||||
btn_save: 1
|
||||
btn_goto: 3
|
||||
</rosparam>
|
||||
</group>
|
||||
|
||||
<!-- ROS navigation stack move_base -->
|
||||
<group ns="planner">
|
||||
<remap from="openni_points" to="/planner_cloud"/>
|
||||
<remap from="base_scan" to="/base_scan"/>
|
||||
<remap from="map" to="/map"/>
|
||||
<remap from="move_base_simple/goal" to="/planner_goal"/>
|
||||
<include file="$(find az3_navigation)/launch/az3_move_base_slam.launch"/>
|
||||
|
||||
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
|
||||
<rosparam file="$(find rtabmap)/launch/config/azimut3/costmap_common_params.yaml" command="load" ns="global_costmap" />
|
||||
<rosparam file="$(find rtabmap)/launch/config/azimut3/costmap_common_params.yaml" command="load" ns="local_costmap" />
|
||||
<rosparam file="$(find rtabmap)/launch/config/azimut3/local_costmap_params.yaml" command="load" />
|
||||
<rosparam file="$(find rtabmap)/launch/config/azimut3/global_costmap_params.yaml" command="load" />
|
||||
<rosparam file="$(find rtabmap)/launch/config/azimut3/base_local_planner_params.yaml" command="load" />
|
||||
</node>
|
||||
|
||||
<param name="cmd_vel/abtr_priority" value="10"/>
|
||||
</group>
|
||||
|
||||
@@ -42,6 +39,7 @@
|
||||
<remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
|
||||
</node>
|
||||
|
||||
<!-- Arbitration between teleop and planner -->
|
||||
<node name="register_cmd_eta" pkg="abtr_priority" type="register"
|
||||
args="/cmd_eta /teleop/cmd_eta"/>
|
||||
<node name="register_cmd_vel" pkg="abtr_priority" type="register"
|
||||
@@ -87,14 +85,11 @@
|
||||
<remap from="right/image_raw" to="right/image_raw_throttle"/>
|
||||
<remap from="right/camera_info" to="right/camera_info_throttle"/>
|
||||
</node>
|
||||
<node pkg="nodelet" type="nodelet" name="disparity_odom_sync" args="load rtabmap/disparity_odom_sync stereo_nodelet">
|
||||
<remap from="disparity" to="disparity"/>
|
||||
<remap from="camera_info" to="right/camera_info_throttle"/>
|
||||
<remap from="odom" to="/stereo_odometer/odometry"/>
|
||||
</node>
|
||||
|
||||
<!-- Create point cloud for the planner -->
|
||||
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap/point_cloud_xyz stereo_nodelet">
|
||||
<remap from="disparity/image" to="disparity_sync"/>
|
||||
<remap from="disparity/camera_info" to="right/camera_info_throttle_sync"/>
|
||||
<remap from="disparity/image" to="disparity"/>
|
||||
<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
|
||||
<remap from="cloud" to="/planner_cloud"/>
|
||||
|
||||
<param name="voxel_size" type="double" value="0.05"/>
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
TrajectoryPlannerROS:
|
||||
|
||||
# Current limits based on AZ3 standalone configuration.
|
||||
acc_lim_x: 0.75
|
||||
acc_lim_y: 0.75
|
||||
acc_lim_th: 4.00
|
||||
# min_vel_x and max_rotational_vel were set to keep the ICR at
|
||||
# minimal distance of 0.48 m.
|
||||
# Basically, max_rotational_vel * rho_min <= min_vel_x
|
||||
max_vel_x: 0.500
|
||||
min_vel_x: 0.212
|
||||
max_rotational_vel: 0.550
|
||||
min_in_place_rotational_vel: 0.15
|
||||
escape_vel: -0.10
|
||||
holonomic_robot: false
|
||||
|
||||
xy_goal_tolerance: 0.20
|
||||
yaw_goal_tolerance: 0.20
|
||||
|
||||
sim_time: 1.7
|
||||
sim_granularity: 0.025
|
||||
vx_samples: 3
|
||||
vtheta_samples: 3
|
||||
vtheta_samples: 20
|
||||
|
||||
goal_distance_bias: 0.8
|
||||
path_distance_bias: 0.6
|
||||
occdist_scale: 0.01
|
||||
heading_lookahead: 0.325
|
||||
dwa: true
|
||||
|
||||
oscillation_reset_dist: 0.05
|
||||
@@ -0,0 +1,11 @@
|
||||
obstacle_range: 2.5
|
||||
raytrace_range: 3.0
|
||||
footprint: [[ 0.3, 0.3], [-0.3, 0.3], [-0.3, -0.3], [ 0.3, -0.3]]
|
||||
#robot_radius: ir_of_robot
|
||||
inflation_radius: 0.55
|
||||
|
||||
observation_sources: laser_scan_sensor point_cloud_sensor
|
||||
|
||||
laser_scan_sensor: {data_type: LaserScan, topic: base_scan, marking: true, clearing: true}
|
||||
|
||||
point_cloud_sensor: {data_type: PointCloud2, topic: openni_points, marking: true, clearing: true}
|
||||
@@ -0,0 +1,5 @@
|
||||
global_costmap:
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
update_frequency: 5.0
|
||||
static_map: true
|
||||
@@ -0,0 +1,10 @@
|
||||
local_costmap:
|
||||
global_frame: odom
|
||||
robot_base_frame: base_footprint
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
static_map: false
|
||||
rolling_window: true
|
||||
width: 4.0
|
||||
height: 4.0
|
||||
resolution: 0.05
|
||||
+1
-9
@@ -22,15 +22,7 @@
|
||||
This is my nodelet.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
<class name="rtabmap/disparity_odom_sync"
|
||||
type="rtabmap::DisparityOdomSyncNodelet"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
<description>
|
||||
This is my nodelet.
|
||||
</description>
|
||||
</class>
|
||||
|
||||
|
||||
<class name="rtabmap/point_cloud_xyzrgb"
|
||||
type="rtabmap::PointCloudXYZRGB"
|
||||
base_class_type="nodelet::Nodelet">
|
||||
|
||||
@@ -1,129 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#include "ros/ros.h"
|
||||
#include "pluginlib/class_list_macros.h"
|
||||
#include "nodelet/nodelet.h"
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/time_synchronizer.h>
|
||||
#include <message_filters/sync_policies/approximate_time.h>
|
||||
#include <message_filters/sync_policies/exact_time.h>
|
||||
|
||||
#include <stereo_msgs/DisparityImage.h>
|
||||
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class DisparityOdomSyncNodelet : public nodelet::Nodelet
|
||||
{
|
||||
public:
|
||||
//Constructor
|
||||
DisparityOdomSyncNodelet():
|
||||
approxSync_(0),
|
||||
exactSync_(0)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~DisparityOdomSyncNodelet()
|
||||
{
|
||||
if(approxSync_)
|
||||
delete approxSync_;
|
||||
if(exactSync_)
|
||||
delete exactSync_;
|
||||
}
|
||||
|
||||
private:
|
||||
virtual void onInit()
|
||||
{
|
||||
ros::NodeHandle& nh = getNodeHandle();
|
||||
ros::NodeHandle& private_nh = getPrivateNodeHandle();
|
||||
|
||||
int queueSize = 10;
|
||||
bool approxSync = false;
|
||||
private_nh.param("queue_size", queueSize, queueSize);
|
||||
private_nh.param("approx_sync", approxSync, approxSync);
|
||||
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), disparitySub_, infoSub_, odomSub_);
|
||||
approxSync_->registerCallback(boost::bind(&DisparityOdomSyncNodelet::callback, this, _1, _2, _3));
|
||||
}
|
||||
else
|
||||
{
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), disparitySub_, infoSub_, odomSub_);
|
||||
exactSync_->registerCallback(boost::bind(&DisparityOdomSyncNodelet::callback, this, _1, _2, _3));
|
||||
}
|
||||
|
||||
disparitySub_.subscribe(nh, "disparity", 1);
|
||||
infoSub_.subscribe(nh, "camera_info", 1);
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
|
||||
disparityPub_ = nh.advertise<stereo_msgs::DisparityImage>(nh.resolveName("disparity")+"_sync", 1);
|
||||
infoPub_ = nh.advertise<sensor_msgs::CameraInfo>(nh.resolveName("camera_info")+"_sync", 1);
|
||||
odomPub_ = nh.advertise<nav_msgs::Odometry>(nh.resolveName("odom")+"_sync", 1);
|
||||
};
|
||||
|
||||
void callback(const stereo_msgs::DisparityImageConstPtr& image,
|
||||
const sensor_msgs::CameraInfoConstPtr& camInfo,
|
||||
const nav_msgs::OdometryConstPtr & odom)
|
||||
{
|
||||
if(disparityPub_.getNumSubscribers())
|
||||
{
|
||||
disparityPub_.publish(image);
|
||||
}
|
||||
if(infoPub_.getNumSubscribers())
|
||||
{
|
||||
infoPub_.publish(camInfo);
|
||||
}
|
||||
if(odomPub_.getNumSubscribers())
|
||||
{
|
||||
odomPub_.publish(odom);
|
||||
}
|
||||
}
|
||||
|
||||
ros::Publisher disparityPub_;
|
||||
ros::Publisher infoPub_;
|
||||
ros::Publisher odomPub_;
|
||||
|
||||
message_filters::Subscriber<stereo_msgs::DisparityImage> disparitySub_;
|
||||
message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub_;
|
||||
message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
|
||||
typedef message_filters::sync_policies::ApproximateTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, nav_msgs::Odometry> MyApproxSyncPolicy;
|
||||
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
|
||||
typedef message_filters::sync_policies::ExactTime<stereo_msgs::DisparityImage, sensor_msgs::CameraInfo, nav_msgs::Odometry> MyExactSyncPolicy;
|
||||
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap::DisparityOdomSyncNodelet, nodelet::Nodelet);
|
||||
}
|
||||
Reference in New Issue
Block a user