Gazebo | Ignition | Community
Ask Your Question
0

Reading sonar sensor data in ModelPlugin without ROS

asked 2021-01-04 15:22:25 -0500

akshat gravatar image

updated 2021-01-09 18:33:55 -0500

Hi, This is my first question here. Pardon me for any mistakes.

I have spent some time looking this over the internet but could not find a working solution yet. Documentation doesn't have much on Sonar sensors..

I am trying to read the sonar sensor range values which the sensor plugin, SonarPlugin.cc gives me and publishes to the topic /gazebo/mtp_world/mtp_robot/left_sonar/mtp_sonar_sensor_left/sonar in the robot's model plugin, move_bot.cc using a transport node and a subscriber

I got this topic from gz topic -l and doing gz topic -e <this-topic-name> gives the required range outputs along with other sonar values.

I have a very basic understanding of topics work. The message returned from the subscriber is NULL for this topic.

Here are the relevant code snippets:

    class MoveBot : public ModelPlugin {
        private: physics::WorldPtr world;
        private: physics::ModelPtr model;
        private: event::ConnectionPtr updateConnection;
        private: physics::JointPtr leftJoint;
        private: physics::JointPtr rightJoint;
        private: common::Time startTime;
        private: transport::SubscriberPtr sub;

        public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /* _sdf */) {
            this->model = _parent;
            this->world = this->model->GetWorld();
            this->leftJoint = this->model->GetJoint("left_wheel_hinge");
            this->rightJoint = this->model->GetJoint("right_wheel_hinge");

            transport::NodePtr node(new transport::Node());
            node->Init();
            this->sonarTopicLeft = 
                "/gazebo/mtp_world/mtp_robot/left_sonar/mtp_sonar_sensor_left/sonar";
            this->sub = node->Subscribe(this->sonarTopicLeft, &MoveBot::cb, this);
            this->updateConnection =
                event::Events::ConnectWorldUpdateBegin(std::bind(&MoveBot::OnUpdate, this));
        }

        private: void cb(ConstSonarPtr &_msg) {
            std::cout<< "callback!!!!!!!" << std::endl;
            if (_msg == NULL) {
                std::cout<< "NULL!!!!!!!" << std::endl;
                return;
            }
            std::cout << "Message: " << _msg->range() << std::endl;
        }
        private: void onUpdate() {
            // Some other code
        }
}

I am expecting the cb(ConstSonarPtr &_msg) callback to print the range values but I only get NULL!!!!!!! printed.

Any help would be greatly appreciated! Thank you.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-01-11 19:36:59 -0500

akshat gravatar image

updated 2021-01-11 20:15:44 -0500

Solved it!

The problem was the wrong type of argument in the callback function cb. i.e. ConstSonarPtr.

I did a gz topic -i <topic-name> and found that the msg type is actually ConstSonarStampedPtr.

$ gz topic -i /gazebo/mtp_world/mtp_robot/left_sonar/mtp_sonar_sensor_left/sonar
Type: gazebo.msgs.SonarStamped

Publishers:
        192.168.1.4:39483

Subscribers:
    192.168.1.4:39483

FInally had to make the following changes to the cb callback to get the range values:

private: void cb(ConstSonarStampedPtr &_msg) {
    if (_msg == NULL) {
        return;
    }
    gazebo::msgs::Sonar sonarMsg = _msg->sonar();
    std::cout << "Range: " << sonarMsg.range() << std::endl;
}

Thanks to a post here somewhere, I found about gz topic -i <topic-name> and was able to solve the problem.

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2021-01-04 15:04:01 -0500

Seen: 64 times

Last updated: Jan 11