Help with ros2 action_client

[Michael Wimble]

I am trying to create a behavior tree node that moves the robot to a goal. The following snippet fails, always timing out. If I use the command line to call the move_to_pose action, or use rqt to do the same, it succeeds. Any clues why my code fails to create a client? Note, I also tried this without supplying the opt argument, which is here because I though the quality of service (qos) might be the reason for the failure, but it fails the same way with and without the qos options.

MoveToPose::MoveToPose(const std::string& name,

const BT::NodeConfiguration& config)

: AsyncActionNode(name, config), aborted_(false) {

std::cout << "[MoveToPose] constructed" << std::endl;

}

BT::NodeStatus MoveToPose::tick() {

auto qos = rclcpp::QoS(

rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10));

qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);

qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);

qos.avoid_ros_namespace_conventions(false);

rcl_action_client_options_t opts;

opts.cancel_service_qos = qos;

opts.feedback_topic_qos = qos;

opts.goal_service_qos = qos;

opts.goal_service_qos = qos;

opts.result_service_qos = qos;

opts.status_topic_qos = qos;

std::cout << "[MoveToPose] tick() called" << std::endl;

auto node = rclcpp::Node::make_shared("move_to_pose_action_client");

auto action_client =

rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(

node, "move_to_pose", nullptr, opts);

RCLCPP_INFO(node->get_logger(),

"[MoveToPose] action client created, waiting for server");

if (!action_client->wait_for_action_server(std::chrono::seconds(20))) {

RCLCPP_ERROR(node->get_logger(),

"Action server not available after waiting");

return BT::NodeStatus::FAILURE;

}

[Scott Horton]

Hi Michael,

Were you able to get over that hurdle?

If not, if you can post/send a streamlined version of your BT node, I'll be happy to try it with in my setup.

Also, what does the main() for the BT node look like?  Are you sure the node is spinning some?

Scott

[Michael Wimble]

I got past that hurdle, it was a typo in a string constant. My “move_to_pose” should have been “navigate_to_pose”. There was nothing in the software (like debugging options) to help me diagnose this, it just took me a few hours of head scratching before I noticed what was wrong.

Now my problem is in trying to translate a point that my lidar detects in the base_link frame to a point in the map frame, which is required for the navigate_to_pose action to work. I have the code but there is a linker error which so far I have been unable to resolve, even grepping “.so” files to look for a definition of the required api. But, I’ll go find the original tf2 ROS2 code and see if I can dig through the nested calls there to figure out what the issue it. It probably has something to do with API changes in Galactic.

Thanks for asking.

Oh, and I’ve tried maybe four ways to marry BehaviorTree.cpp with navigate_to_pose and most didn’t work—it’s a problem I’ll come back to some day in the future. For now, I just cannot get any feedback callbacks while the robot is moving, but I can live with that for today.

--
You received this message because you are subscribed to the Google Groups "HomeBrew Robotics Club" group.
To unsubscribe from this group and stop receiving emails from it, send an email to hbrobotics+...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/0a5653d7-36eb-47f0-bacc-f7e0b454e847n%40googlegroups.com.

[Scott Horton]

That's good to hear you found it.  Those are frustrating when they burn several hours, and they kinda squeeze the fun out of this stuff.  I seem to hit those when I'm trying to do hobby work with an already worn-out brain from the day job.

I'm sure you've found plenty of examples for that translation functionality you are trying to get to build, but if you need another, you can take a look at my 'tracked_object_mapper' repo.  It does mapping from the oakd frame of Elsabot to the map frame.

Oh, and I’ve tried maybe four ways to marry BehaviorTree.cpp with navigate_to_pose and most didn’t work—it’s a problem I’ll come back to some day in the future. For now, I just cannot get any feedback callbacks while the robot is moving, but I can live with that for today.

I'm not sure I completely understand what you mean.  Are you saying you are wanting to monitor the current progress of the nav-to-pose?  Maybe use another tree node to get the current pose and save on the blackboard which can then be consumed by another tree node that cares/monitors the progress.

Scott

[Michael Wimble]

On Jan 7, 2022, at 8:36 PM, Scott Horton <horton...@gmail.com> wrote:

I'm not sure I completely understand what you mean.  Are you saying you are wanting to monitor the current progress of the nav-to-pose?  Maybe use another tree node to get the current pose and save on the blackboard which can then be consumed by another tree node that cares/monitors the progress.

Pretty much what you said. First, my code is in a MoveToPose BehaviorTree node, and I was just trying to see that I had the code correct to get the callbacks as an action client. But I also thought I might include a sanity check that looks at progress and if the robot seems to be not making much progress, the node would cancel the action. But I could also use the feedback to generate another BT condition node that would be something like RobitIsMakingProgress.

It occurs to me now that I never looked to see if the navigate_to_pose action server actually produced any feedback. Maybe my code was okay all along but there just wasn’t any feedback provided by the action.

Thanks for the pointer to your code. In the past, I looked at a bunch of your code to see how you implemented behavior tree nodes, now I’ll go back and look how you do the transform.