[Unit 9] What's wrong with my solution?

[francesco.mttl]

This is the solution I proposed for the unit final quiz.

#include "ros/duration.h"
#include "ros/init.h"
#include "ros/publisher.h"
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>
#include <actions_quiz/CustomActionMsgAction.h>
#include <actions_quiz/CustomActionMsgFeedback.h>
#include <actions_quiz/CustomActionMsgGoal.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>

class Handler {

protected:
  ros::NodeHandle nh_;

  actionlib::SimpleActionServer<actions_quiz::CustomActionMsgAction> as_;

  ros::Publisher take_off_pub =
      nh_.advertise<std_msgs::Empty>("/drone/takeoff", 1000);
  ros::Publisher land_pub = nh_.advertise<std_msgs::Empty>("/drone/land", 1000);
  std_msgs::Empty empty;

public:
  Handler(std::string name)
      :

        as_(nh_, name, boost::bind(&Handler::execute, this, _1), false) {
    as_.start();
  }

  void execute(const actions_quiz::CustomActionMsgGoalConstPtr &goal) {
    ros::Rate loop_rate(2);
    ROS_INFO_STREAM(" Received " << goal->goal);
    if (goal->goal == "TAKEOFF") {
      take_off_pub.publish(empty);
    } else if (goal->goal == "LAND") {
      land_pub.publish(empty);
    }

    actions_quiz::CustomActionMsgFeedback feedback;
    feedback.feedback = goal->goal;
    ros::spinOnce();
    for (int i = 0; i < 4; ++i) {
      ros::spinOnce();
      loop_rate.sleep();
      as_.publishFeedback(feedback);
    }

    ROS_INFO_STREAM("Done");
    as_.setSucceeded();
  }
};

int main(int argc, char **argv) {
  ros::init(argc, argv, "action_custom_msg_as_node");

  Handler handler("action_custom_msg_as");
  ros::spin();

  return 0;
}

This is what i get from autocorrection:

✔ [18:52:16] [info] Checking that the TAKEOFF call works... (mark: 6.0)

✖ [18:52:52] [assess] Drone did not take off. C'mon, even grandpas can do this!
- Is your code taking the drone off correctly?
- Is your code landing the drone after taking it off? It should NOT. You should use the LAND call for that. (mark: 6.0)

✔ [18:52:55] [info] Checking that the LAND call works... (mark: 6.0)

✖ [18:53:35] [assess] Drone was not landed. You intend to keep it hanging in the air?
- Is your code landing the drone correctly?
- Is your code taking the drone off after landing it? It should NOT. You should use the TAKEOFF call for that. (mark: 6.0)

The drone lands and take off correctly by calling:

rostopic pub /action_custom_msg_as/goal actions_quiz/CustomActionMsgActionGoal "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
goal_id:
  stamp:
    secs: 0
    nsecs: 0
  id: ''
goal:
  goal: 'LAND'"

The autocorrection claims it does not. What am I doing wrong?
Thanks,

[girishkumar.kannan]

Hi @francesco.mttl ,

Your code seems fine. Except for few changes.
You can set loop rate to 1, so the for loop executes for 4 seconds for take off / land. With loop rate 2, you will sleep for 2 seconds, so you will wait for 8 seconds in total.

If I remember correctly, you are supposed to send feedback messages as taking off and landing.
By assigning goal->goal to feedback.feedback, your feedback messages become either TAKEOFF or LAND, which are wrong.

Also, you do not need that FibonacciAction include line anymore.

Let me know if this fixed your problem.

Regards,
Girish

[francesco.mttl]

Thanks for the support!