i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated goal action feedback

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-06 16:28:28 -05:00
parent 8100a38688
commit ddddd70d66
2 changed files with 86 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/azimut3/config/azimut3_nav.rviz
+54 -15
View File
@@ -6,9 +6,6 @@ Panels:
Expanded:
- /Global Options1
- /TF1/Frames1
- /MapCloud1
- /Path1
- /Path2
Splitter Ratio: 0.601881
Tree Height: 192
- Class: rviz/Selection
@@ -22,7 +19,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
SyncSource: Info
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
@@ -71,6 +68,8 @@ Visualization Manager:
Value: true
camera_rgb_optical_frame:
Value: true
map:
Value: true
odom:
Value: true
wheelLB_linkWheel_link:
@@ -95,7 +94,31 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
{}
map:
odom:
base_footprint:
base_link:
base_laser_link:
{}
camera_link:
camera_depth_frame:
camera_depth_optical_frame:
{}
camera_rgb_frame:
camera_rgb_optical_frame:
{}
wheelLB_linkWheel_link:
wheelLB_wheel_link:
{}
wheelLF_linkWheel_link:
wheelLF_wheel_link:
{}
wheelRB_linkWheel_link:
wheelRB_wheel_link:
{}
wheelRF_linkWheel_link:
wheelRF_wheel_link:
{}
Update Interval: 0
Value: true
- Alpha: 1
@@ -139,18 +162,18 @@ Visualization Manager:
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Enabled: false
Name: Map
Topic: /planner/move_base/local_costmap/costmap
Value: true
Value: false
- Alpha: 0.7
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Enabled: false
Enabled: true
Name: Map
Topic: /planner/move_base/global_costmap/costmap
Value: false
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
@@ -284,7 +307,7 @@ Visualization Manager:
Shaft Length: 1
Shaft Radius: 0.05
Shape: Arrow
Topic: /planner_goal
Topic: /rtabmap/goal
Value: true
- Alpha: 1
Buffer Length: 1
@@ -333,11 +356,27 @@ Visualization Manager:
Value: false
- Alpha: 1
Class: rtabmap_ros/MapGraph
Color: 255; 0; 0
Color: 0; 0; 255
Enabled: true
Name: MapGraph
Topic: /rtabmap/mapData_relay
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Name: Path
Topic: /rtabmap/global_path
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 85; 255; 255
Enabled: true
Name: Path
Topic: /rtabmap/local_path
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -358,7 +397,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 8.53098
Distance: 7.4465
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
@@ -370,10 +409,10 @@ Visualization Manager:
Z: -0.156054
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.9304
Pitch: 1.4204
Target Frame: base_footprint
Value: Orbit (rviz)
Yaw: 2.26538
Yaw: 2.93539
Saved: ~
Window Geometry:
Displays:
@@ -393,5 +432,5 @@ Window Geometry:
Views:
collapsed: false
Width: 714
X: 928
X: 921
Y: 32
src/CoreWrapper.cpp
+32 -8
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Bool.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/Parameters.h>
@@ -138,7 +139,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
goalGlobalSub_ = nh.subscribe("goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1);
goalReachedPub_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1);
globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
@@ -945,7 +946,9 @@ void CoreWrapper::process(
ROS_INFO("Planning: Publishing goal reached!");
if(goalReachedPub_.getNumSubscribers())
{
goalReachedPub_.publish(std_msgs::Empty());
std_msgs::Bool result;
result.data = true;
goalReachedPub_.publish(result);
}
currentMetricGoal_.setNull();
}
@@ -1005,6 +1008,12 @@ void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> >
{
ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathCurrentGoalId());
rtabmap_.clearPath();
if(goalReachedPub_.getNumSubscribers())
{
std_msgs::Bool result;
result.data = false;
goalReachedPub_.publish(result);
}
}
else
{
@@ -1044,7 +1053,13 @@ void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> >
else
{
ROS_WARN("Planning: Cannot compute a path (or goal is already reached)!");
goalReachedPub_.publish(std_msgs::Empty());
rtabmap_.clearPath();
if(goalReachedPub_.getNumSubscribers())
{
std_msgs::Bool result;
result.data = false;
goalReachedPub_.publish(result);
}
}
}
@@ -1428,25 +1443,34 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
{
if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("Planning: move_base success");
ROS_INFO("Planning: move_base success!");
}
else
{
ROS_INFO("Planning: move_base failed for some reason.");
ROS_ERROR("Planning: move_base failed for some reason. Aborting the plan...");
}
rtabmap_.clearPath();
currentMetricGoal_.setNull();
if(goalReachedPub_.getNumSubscribers())
{
std_msgs::Bool result;
result.data = state == actionlib::SimpleClientGoalState::SUCCEEDED;
goalReachedPub_.publish(result);
}
}
// Called once when the goal becomes active
void CoreWrapper::goalActiveCb()
{
ROS_INFO("Planning: Goal just went active");
//ROS_INFO("Planning: Goal just went active");
}
// Called every time feedback is received for the goal
void CoreWrapper::goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
{
Transform basePosition = rtabmap_ros::transformFromPoseMsg(feedback->base_position.pose);
ROS_INFO("Planning: feedback base_position = %s", basePosition.prettyPrint().c_str());
//Transform basePosition = rtabmap_ros::transformFromPoseMsg(feedback->base_position.pose);
//ROS_INFO("Planning: feedback base_position = %s", basePosition.prettyPrint().c_str());
}
void CoreWrapper::publishLocalPath(const ros::Time & stamp)