Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Nav2-rviz-plugin changes. #937

Merged
merged 3 commits into from
Jul 24, 2019
Merged

Nav2-rviz-plugin changes. #937

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5d3c0cc
I added a timeout to spin_until_future_complete, so rviz doesn't get …
mlherd Jul 16, 2019
f8ec2f8
made the code cleaner and changed the error messages. Reduced the tim…
mlherd Jul 20, 2019
df14336
added timeout parameter
mlherd Jul 22, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion nav2_rviz_plugins/include/nav2_rviz_plugins/navigation_dialog.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ class NavigationDialog : public QDialog
public:
explicit NavigationDialog(QWidget * parent = 0);

void startNavigation(double x, double y, double theta, std::string & frame);
bool startNavigation(double x, double y, double theta, std::string & frame);

protected:
void timerEvent(QTimerEvent * event);
Expand All @@ -45,6 +45,9 @@ private slots:

private:
using GoalHandle = rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;

// time out for wait_for_action_server and send goal (seconds)
int timeout = 3;

// The (non-spinning) client node used to invoke the action client
rclcpp::Node::SharedPtr client_node_;
Expand Down
10 changes: 5 additions & 5 deletions nav2_rviz_plugins/src/goal_tool.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,11 +47,11 @@ void
GoalTool::onPoseSet(double x, double y, double theta)
{
std::string fixed_frame = context_->getFixedFrame().toStdString();

navigation_dialog_->startNavigation(x, y, theta, fixed_frame);
navigation_dialog_->show();
navigation_dialog_->raise();
navigation_dialog_->activateWindow();
if (navigation_dialog_->startNavigation(x, y, theta, fixed_frame)) {
navigation_dialog_->show();
navigation_dialog_->raise();
navigation_dialog_->activateWindow();
}
}

} // namespace nav2_rviz_plugins
Expand Down
19 changes: 11 additions & 8 deletions nav2_rviz_plugins/src/navigation_dialog.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ NavigationDialog::orientationAroundZAxis(double angle)
return tf2::toMsg(q);
}

void
bool
NavigationDialog::startNavigation(double x, double y, double theta, std::string & frame)
{
auto pose = geometry_msgs::msg::PoseStamped();
Expand All @@ -114,11 +114,11 @@ NavigationDialog::startNavigation(double x, double y, double theta, std::string
pose.pose.position.z = 0.0;
pose.pose.orientation = orientationAroundZAxis(theta);

auto is_action_server_ready = action_client_->wait_for_action_server(std::chrono::seconds(5));
auto is_action_server_ready = action_client_->wait_for_action_server(std::chrono::seconds(timeout));
if (!is_action_server_ready) {
RCLCPP_ERROR(client_node_->get_logger(), "NavigateToPose action server is not available."
" Is the initial pose set?");
return;
" Check if the simulator or the robot is running. Make sure the initial pose is set.");
return false;
}

// Send the goal pose
Expand All @@ -130,19 +130,22 @@ NavigationDialog::startNavigation(double x, double y, double theta, std::string
send_goal_options.result_callback = [](auto) {};

auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
if (rclcpp::spin_until_future_complete(client_node_, future_goal_handle) !=
if (rclcpp::spin_until_future_complete(client_node_,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What's special about 5? This should be parameterized

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nothing is special about 5. I don't think there is any perfect number for these timeouts. I will make it a parameter. Should there be two separate parameters for each timeout?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It looks like both of thost 5s are accomplishing the same thing: checking if we are able to get some basic response from the action server, so I think 1 parameter is all that is needed.

Make sure when you add a parameter you add the field to the nav2_params file so that we have a fully-described parameter file, there's a number of them missing right now (and on my queue to deal with) but it would make my life easier if it more were there than not.

Copy link
Member

@SteveMacenski SteveMacenski Jul 17, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for your help in my on-going war against magic numbers

image

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

haha.. Thanks for your guidance. From now on, I will be after the magic numbers in our stack :)

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@SteveMacenski Regarding your full-described parameter file... FYI - I've written a parameter dumping utility. One can launch the nav2 stack and then invoke startup to take all of the nodes through configure and activate states. Then, run this utility. It will iterate over all of the nodes and dump every parameter in either MD or YAML formats, as specified with a command-line parameter. It's useful to document the default parameter values. I was thinking that one could start the stack without any nav2_params.yaml file and then dump the parameters. We'd probably have to specify the BT navigation XML file and map file from the launch script in this case.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@SteveMacenski I left the timeouts as hard coded values for now. I think we agreed on we should parameterize all the hard codded magic numbers in the stack during our previous weekly work group meeting, but we haven't decided how we are going to do these changes yet. Since I think the Rviz crashing issue can be a blocking problem for some users, we can merge this PR if everything looks good in my changes. I will submit a different issue "parameterize all the hard coded values in the stack" for this discussion.

future_goal_handle, std::chrono::seconds(timeout)) !=
rclcpp::executor::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed");
return;
RCLCPP_ERROR(client_node_->get_logger(), "Send goal call failed."
" Check if the simulator or the robot is running. Make sure the initial pose is set.");
return false;
}

// Get the goal handle and save so that we can check on completion in the timer callback
goal_handle_ = future_goal_handle.get();
if (!goal_handle_) {
RCLCPP_ERROR(client_node_->get_logger(), "Goal was rejected by server");
return;
return false;
}

timer_.start(100, this);
return true;
}