mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Pose goals can be sent directly to rtabmap node, Added SetGoal service (set target node id)
This commit is contained in:
Mathieu Labbe
@@ -52,6 +52,7 @@ add_message_files(
|
||||
GetMap.srv
|
||||
PublishMap.srv
|
||||
ResetPose.srv
|
||||
SetGoal.srv
|
||||
)
|
||||
|
||||
## Generate added messages and services with any dependencies listed here
|
||||
|
||||
@@ -16,22 +16,9 @@
|
||||
|
||||
<!-- Visualisation RVIZ -->
|
||||
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
|
||||
|
||||
<!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
|
||||
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
|
||||
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
|
||||
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
|
||||
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
|
||||
<remap from="cloud" to="voxel_cloud" />
|
||||
|
||||
<param name="queue_size" type="int" value="10"/>
|
||||
<param name="voxel_size" type="double" value="0.01"/>
|
||||
</node>
|
||||
|
||||
<!-- use a relay on this machine, same for images -->
|
||||
<node if="$(arg rviz)" name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay"/>
|
||||
<node if="$(arg rviz)" name="camera_info_relay" type="relay" pkg="topic_tools" args="/camera/data_throttled_camera_info /camera/data_throttled_camera_info_relay"/>
|
||||
<node if="$(arg rviz)" name="republish_rgb" type="republish" pkg="image_transport" args="theora in:=/camera/data_throttled_image raw out:=/camera/data_throttled_image_relay" />
|
||||
<node if="$(arg rviz)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=/camera/data_throttled_image_depth raw out:=/camera/data_throttled_image_depth_relay" />
|
||||
|
||||
<node if="$(arg rviz)" name="mapData_relay" type="relay" pkg="topic_tools" args="/rtabmap/mapData /rtabmap/mapData_relay">
|
||||
<param name="lazy" type="bool" value="true"/>
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
@@ -7,73 +7,9 @@
|
||||
<!-- AZIMUT 3 bringup: launch motors/odometry, laser scan and openni -->
|
||||
<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"/>
|
||||
<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>
|
||||
|
||||
<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"/>
|
||||
<param name="cmd_vel/abtr_priority" value="10"/>
|
||||
</group>
|
||||
|
||||
<node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
|
||||
<remap from="abtr_cmd_eta" to="/base_controller/cmd_eta"/>
|
||||
</node>
|
||||
|
||||
<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"
|
||||
args="/cmd_vel /planner/cmd_vel"/>
|
||||
|
||||
|
||||
<!-- OpenNI -->
|
||||
<include file="$(find rtabmap_ros)/launch/azimut3/az3_openni.launch"/>
|
||||
|
||||
<!-- Throttling messages -->
|
||||
<group ns="camera">
|
||||
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
|
||||
<param name="max_rate" type="double" value="5.0"/>
|
||||
|
||||
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
|
||||
<remap from="depth/image_in" to="depth_registered/image_raw"/>
|
||||
<remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
|
||||
|
||||
<remap from="rgb/image_out" to="data_throttled_image"/>
|
||||
<remap from="depth/image_out" to="data_throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
|
||||
</node>
|
||||
|
||||
<!-- for the planner -->
|
||||
<node pkg="nodelet" type="nodelet" name="points_xyzrgb_planner" args="load rtabmap_ros/point_cloud_xyzrgb camera_nodelet_manager">
|
||||
<remap from="rgb/image" to="/camera/data_throttled_image"/>
|
||||
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
|
||||
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
|
||||
<remap from="cloud" to="/planner_cloud" />
|
||||
|
||||
<param name="queue_size" type="int" value="10"/>
|
||||
<param name="decimation" type="int" value="4"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
<!-- SLAM (robot side) -->
|
||||
<group ns="rtabmap">
|
||||
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
|
||||
@@ -94,7 +30,6 @@
|
||||
|
||||
<param name="queue_size" type="int" value="10"/>
|
||||
|
||||
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
|
||||
<param name="RGBD/PoseScanMatching" type="string" value="true"/>
|
||||
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
|
||||
|
||||
@@ -136,4 +71,41 @@
|
||||
</node>
|
||||
|
||||
</group>
|
||||
|
||||
<!-- teleop -->
|
||||
<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"/>
|
||||
<param name="cmd_eta/abtr_priority" value="50"/>
|
||||
</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"/>
|
||||
|
||||
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_laser_only.yaml" command="load" />
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/global_costmap_params.yaml" command="load" />
|
||||
<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/base_local_planner_params.yaml" command="load" />
|
||||
</node>
|
||||
|
||||
<param name="cmd_vel/abtr_priority" value="10"/>
|
||||
</group>
|
||||
|
||||
<node name="az3_abtr" pkg="azimut_tools" type="azimut_abtr_priority_node">
|
||||
<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"
|
||||
args="/cmd_vel /planner/cmd_vel"/>
|
||||
|
||||
</launch>
|
||||
|
||||
@@ -6,23 +6,23 @@ Panels:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /TF1/Frames1
|
||||
- /Info1/Status1
|
||||
- /MapGraph1
|
||||
- /MapCloud1
|
||||
- /Path1
|
||||
- /Path2
|
||||
Splitter Ratio: 0.601881
|
||||
Tree Height: 337
|
||||
Tree Height: 437
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
- /Current View1/Focal Point1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: Image
|
||||
SyncSource: ""
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
@@ -127,8 +127,8 @@ Visualization Manager:
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 5.19727
|
||||
Min Value: -0.547517
|
||||
Max Value: 3.46596
|
||||
Min Value: -5.27159e-08
|
||||
Value: true
|
||||
Axis: X
|
||||
Channel Name: intensity
|
||||
@@ -136,7 +136,7 @@ Visualization Manager:
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
@@ -152,7 +152,7 @@ Visualization Manager:
|
||||
Topic: /base_scan
|
||||
Use Fixed Frame: false
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Value: false
|
||||
- Alpha: 0.7
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
@@ -173,10 +173,10 @@ Visualization Manager:
|
||||
Class: rviz/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Name: Map
|
||||
Topic: /planner/move_base/global_costmap/costmap
|
||||
Value: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
@@ -249,7 +249,7 @@ Visualization Manager:
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Enabled: false
|
||||
Image Topic: /camera/data_throttled_image_relay
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
@@ -258,7 +258,7 @@ Visualization Manager:
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Value: true
|
||||
Value: false
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
@@ -384,32 +384,32 @@ Visualization Manager:
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 14.3083
|
||||
Distance: 3.44552
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 1.91382
|
||||
Y: -0.564774
|
||||
Z: 0.661231
|
||||
X: 0.0430748
|
||||
Y: -0.00858197
|
||||
Z: -0.156054
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 1.4054
|
||||
Target Frame: <Fixed Frame>
|
||||
Pitch: 0.9304
|
||||
Target Frame: base_footprint
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.1704
|
||||
Yaw: 2.26538
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 826
|
||||
Hide Left Dock: false
|
||||
collapsed: true
|
||||
Height: 504
|
||||
Hide Left Dock: true
|
||||
Hide Right Dock: false
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000151000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000192000000dd00fffffffb0000000a0049006d00610067006501000001c00000015c0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000002f7000000800000006400ffffff000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002f4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003ad000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd0000000400000000000001510000028ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000028f000000dd00fffffffb0000000a0049006d006100670065000000018a0000012d0000001600fffffffb0000000a0049006d0061006700650000000184000000490000000000000000fb0000000a0049006d006100670065010000027d000000fa0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000002f7000000800000006400ffffff000000010000010f000001b2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001b2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002ca000001b200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
@@ -418,6 +418,6 @@ Window Geometry:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1561
|
||||
X: 42
|
||||
Y: 53
|
||||
Width: 714
|
||||
X: 935
|
||||
Y: 32
|
||||
|
||||
@@ -0,0 +1,33 @@
|
||||
local_costmap:
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
update_frequency: 2.0
|
||||
publish_frequency: 2.0
|
||||
static_map: false
|
||||
rolling_window: true
|
||||
width: 4.0
|
||||
height: 4.0
|
||||
resolution: 0.025
|
||||
origin_x: -2.0
|
||||
origin_y: -2.0
|
||||
|
||||
observation_sources: laser_scan_sensor
|
||||
#observation_sources: laser_scan_sensor point_cloud_sensor
|
||||
|
||||
laser_scan_sensor: {
|
||||
data_type: LaserScan,
|
||||
topic: base_scan,
|
||||
expected_update_rate: 0.2,
|
||||
marking: true,
|
||||
clearing: true}
|
||||
|
||||
# assuming receiving a cloud from rtabmap/obstacles_detection node
|
||||
#point_cloud_sensor: {
|
||||
# sensor_frame: base_footprint,
|
||||
# data_type: PointCloud2,
|
||||
# topic: openni_points,
|
||||
# expected_update_rate: 0.5,
|
||||
# marking: true,
|
||||
# clearing: true,
|
||||
# min_obstacle_height: -99999.0,
|
||||
# max_obstacle_height: 99999.0}
|
||||
+59
-17
@@ -132,8 +132,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
mapGraph_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
|
||||
|
||||
// planning topics
|
||||
goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
|
||||
nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_pose", 1);
|
||||
goalSub_ = nh.subscribe("in_goal", 1, &CoreWrapper::goalCallback, this);
|
||||
goalGlobalSub_ = nh.subscribe("in_goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
|
||||
nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("out_goal", 1);
|
||||
goalReachedPub_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
|
||||
globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
|
||||
localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
|
||||
@@ -280,6 +281,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
|
||||
getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
|
||||
publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
|
||||
setGoalSrv_ = nh.advertiseService("set_goal", &CoreWrapper::setGoalCallback, this);
|
||||
|
||||
setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
|
||||
|
||||
@@ -946,16 +948,22 @@ void CoreWrapper::process(
|
||||
}
|
||||
else
|
||||
{
|
||||
Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathGoalId());
|
||||
Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
|
||||
if(!updatedGoalPose.isNull())
|
||||
{
|
||||
// Adjust the target pose relative to last node
|
||||
if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back())
|
||||
{
|
||||
updatedGoalPose *= rtabmap_.getPathTransformToGoal();
|
||||
}
|
||||
|
||||
// detect if the goal has changed or local map
|
||||
// has changed so much that current goal drifted
|
||||
if(currentMetricGoal_.getDistance(updatedGoalPose) > rtabmap_.getGoalReachedRadius()/2.0f)
|
||||
{
|
||||
currentMetricGoal_ = updatedGoalPose;
|
||||
|
||||
publishGoal(timeNow);
|
||||
publishCurrentGoal(timeNow);
|
||||
}
|
||||
|
||||
// publish local path
|
||||
@@ -963,7 +971,7 @@ void CoreWrapper::process(
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
|
||||
ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -984,19 +992,15 @@ void CoreWrapper::process(
|
||||
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
|
||||
}
|
||||
|
||||
void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
|
||||
void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> > & poses)
|
||||
{
|
||||
currentMetricGoal_.setNull();
|
||||
int id = msg->data;
|
||||
ROS_INFO("Planning: set goal %d", id);
|
||||
|
||||
std::list<std::pair<int, Transform> > poses = rtabmap_.computePath(id);
|
||||
if(poses.size())
|
||||
{
|
||||
currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathGoalId());
|
||||
currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathCurrentGoalId());
|
||||
if(currentMetricGoal_.isNull())
|
||||
{
|
||||
ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
|
||||
ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
|
||||
rtabmap_.clearPath();
|
||||
}
|
||||
else
|
||||
@@ -1013,7 +1017,7 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
|
||||
path.poses.resize(poses.size());
|
||||
int oi = 0;
|
||||
std::stringstream stream;
|
||||
for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
|
||||
for(std::list<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
path.poses[oi].header = path.header;
|
||||
rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
|
||||
@@ -1024,7 +1028,13 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
|
||||
globalPathPub_.publish(path);
|
||||
}
|
||||
|
||||
publishGoal(now);
|
||||
// Adjust the target pose relative to last node
|
||||
if(rtabmap_.getPathCurrentGoalId() == poses.back().first)
|
||||
{
|
||||
currentMetricGoal_ *= rtabmap_.getPathTransformToGoal();
|
||||
}
|
||||
|
||||
publishCurrentGoal(now);
|
||||
publishLocalPath(now);
|
||||
}
|
||||
}
|
||||
@@ -1035,6 +1045,30 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
|
||||
}
|
||||
}
|
||||
|
||||
void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
|
||||
{
|
||||
Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
|
||||
if(targetPose.isNull())
|
||||
{
|
||||
ROS_ERROR("Pose received is null!");
|
||||
return;
|
||||
}
|
||||
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
|
||||
goalCommonCallback(rtabmap_.computePath(targetPose, false));
|
||||
}
|
||||
|
||||
void CoreWrapper::goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
|
||||
{
|
||||
Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
|
||||
if(targetPose.isNull())
|
||||
{
|
||||
ROS_ERROR("Pose received is null!");
|
||||
return;
|
||||
}
|
||||
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
|
||||
goalCommonCallback(rtabmap_.computePath(targetPose, true));
|
||||
}
|
||||
|
||||
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
|
||||
@@ -1278,6 +1312,14 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
|
||||
{
|
||||
int id = req.target_node_id;
|
||||
ROS_INFO("Planning: set goal %d", id);
|
||||
goalCommonCallback(rtabmap_.computePath(id, req.in_global_graph));
|
||||
return !currentMetricGoal_.isNull();
|
||||
}
|
||||
|
||||
void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp)
|
||||
{
|
||||
if(infoPub_.getNumSubscribers())
|
||||
@@ -1331,19 +1373,19 @@ void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp
|
||||
}
|
||||
}
|
||||
|
||||
void CoreWrapper::publishGoal(const ros::Time & stamp)
|
||||
void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
|
||||
{
|
||||
if(!currentMetricGoal_.isNull())
|
||||
{
|
||||
ROS_INFO("Planning: Publishing next goal: Location %d pose=%s",
|
||||
rtabmap_.getPathGoalId(), currentMetricGoal_.prettyPrint().c_str());
|
||||
rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str());
|
||||
if(nextMetricGoalPub_.getNumSubscribers())
|
||||
{
|
||||
geometry_msgs::PoseStamped goalMsg;
|
||||
goalMsg.header.frame_id = mapFrameId_;
|
||||
goalMsg.header.stamp = ros::Time::now();
|
||||
rtabmap_ros::transformToPoseMsg(currentMetricGoal_, goalMsg.pose);
|
||||
ROS_INFO("Publishing next goal: %d", rtabmap_.getPathGoalId());
|
||||
ROS_INFO("Publishing next goal: %d", rtabmap_.getPathCurrentGoalId());
|
||||
nextMetricGoalPub_.publish(goalMsg);
|
||||
}
|
||||
}
|
||||
|
||||
+10
-3
@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include "rtabmap_ros/GetMap.h"
|
||||
#include "rtabmap_ros/PublishMap.h"
|
||||
#include "rtabmap_ros/SetGoal.h"
|
||||
|
||||
#include <message_filters/subscriber.h>
|
||||
#include <message_filters/synchronizer.h>
|
||||
@@ -93,7 +94,10 @@ private:
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const nav_msgs::OdometryConstPtr & odomMsg);
|
||||
void goalNodeCallback(const std_msgs::Int32ConstPtr & msg);
|
||||
|
||||
void goalCommonCallback(const std::list<std::pair<int, rtabmap::Transform> > & poses);
|
||||
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
|
||||
void goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
|
||||
void updateGoal(const ros::Time & stamp);
|
||||
|
||||
void process(
|
||||
@@ -119,6 +123,7 @@ private:
|
||||
bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
|
||||
bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& rep);
|
||||
bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
|
||||
bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
|
||||
|
||||
rtabmap::ParametersMap loadParameters(const std::string & configFile);
|
||||
void saveParameters(const std::string & configFile);
|
||||
@@ -126,7 +131,7 @@ private:
|
||||
void publishLoop(double tfDelay);
|
||||
|
||||
void publishStats(const rtabmap::Statistics & stats, const ros::Time & stamp);
|
||||
void publishGoal(const ros::Time & stamp);
|
||||
void publishCurrentGoal(const ros::Time & stamp);
|
||||
void publishLocalPath(const ros::Time & stamp);
|
||||
|
||||
private:
|
||||
@@ -151,7 +156,8 @@ private:
|
||||
ros::Publisher mapGraph_;
|
||||
|
||||
//Planning stuff
|
||||
ros::Subscriber goalNodeSub_;
|
||||
ros::Subscriber goalSub_;
|
||||
ros::Subscriber goalGlobalSub_;
|
||||
ros::Publisher nextMetricGoalPub_;
|
||||
ros::Publisher goalReachedPub_;
|
||||
ros::Publisher globalPathPub_;
|
||||
@@ -226,6 +232,7 @@ private:
|
||||
ros::ServiceServer setModeMappingSrv_;
|
||||
ros::ServiceServer getMapDataSrv_;
|
||||
ros::ServiceServer publishMapDataSrv_;
|
||||
ros::ServiceServer setGoalSrv_;
|
||||
|
||||
boost::thread* transformThread_;
|
||||
|
||||
|
||||
@@ -95,7 +95,7 @@ public:
|
||||
|
||||
if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
|
||||
{
|
||||
poses = rtabmap::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
|
||||
poses = rtabmap::graph::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
|
||||
}
|
||||
|
||||
if(gridMap_.getNumSubscribers())
|
||||
|
||||
@@ -209,7 +209,7 @@ public:
|
||||
}
|
||||
if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
|
||||
{
|
||||
poses = rtabmap::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
|
||||
poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
|
||||
}
|
||||
|
||||
if(assembledMapClouds_.getNumSubscribers())
|
||||
|
||||
@@ -212,12 +212,12 @@ public:
|
||||
{
|
||||
if(optimizeFromLastNode_)
|
||||
{
|
||||
std::map<int, int> depthGraph = rtabmap::generateDepthGraph(constraints, poses.rbegin()->first);
|
||||
rtabmap::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
|
||||
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(constraints, poses.rbegin()->first);
|
||||
rtabmap::graph::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
|
||||
}
|
||||
else
|
||||
{
|
||||
rtabmap::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
|
||||
rtabmap::graph::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
|
||||
}
|
||||
|
||||
mapToOdomMutex_.lock();
|
||||
|
||||
@@ -325,7 +325,7 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
|
||||
|
||||
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
|
||||
{
|
||||
poses = rtabmap::radiusPosesFiltering(poses,
|
||||
poses = rtabmap::graph::radiusPosesFiltering(poses,
|
||||
node_filtering_radius_->getFloat(),
|
||||
node_filtering_angle_->getFloat()*CV_PI/180.0);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
#request
|
||||
int32 target_node_id
|
||||
bool in_global_graph
|
||||
---
|
||||
#response
|
||||
Reference in New Issue
Block a user