0
$\begingroup$

Gazebo Answers logo

Hi, I am new to gazebo. I had written a Matlab code for nonlinear dynamics of an AUV , now I need to connect my simulink code to gazebo. I am not using the physics of gazebo as the physics is going to be controlled from simulink.

I create a model of the AUV in gazebo and setup the joints for the propeller, rudder and elevator.

image description

However, I am having difficulty understanding the plugin tutorials.

I copy and modified the code from herelink text

I am trying to compile the plugin but I am having error:

[ 50%] Building CXX object CMakeFiles/remusbody.dir/remusbody.cc.o /home/wilmer/catkin_ws/src/plugins/remusbody.cc:5:33: fatal error: geometry_msgs/Twist.h: No such file or directory compilation terminated. CMakeFiles/remusbody.dir/build.make:62: recipe for target 'CMakeFiles/remusbody.dir/remusbody.cc.o' failed make[2]: *** [CMakeFiles/remusbody.dir/remusbody.cc.o] Error 1 CMakeFiles/Makefile2:355: recipe for target 'CMakeFiles/remusbody.dir/all' failed make[1]: *** [CMakeFiles/remusbody.dir/all] Error 2 Makefile:138: recipe for target 'all' failed make: *** [all] Error 2

what I am doing wrong?

I attach this line to package.xml

<build_depend>geometry_msgs</build_depend>

My cmakelist is this

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(gazebo REQUIRED)

find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs)

 add_library(remusbody SHARED remusbody.cc)

 target_link_libraries(remusbody ${GAZEBO_LIBRARIES})

 include_directories(${GAZEBO_INCLUDE_DIRS})

 link_directories(${GAZEBO_LIBRARY_DIRS})

 list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

My plugin code is attached down.

any help is welcome...

#include <stdio.h>

#include <geometry_msgs/Twist.h>
#include "geometry_msgs/PoseStamped.h"

#include "gazebo/gazebo.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/common/common.hh"
#include "math/Vector3.hh"
#include <math/gzmath.hh>

static float Vlin=0.0, Vang=0.0; // set as global 

namespace gazebo
{   
  class ROSModelPlugin : public ModelPlugin
  {   
    public: ROSModelPlugin()
    {
      // Start up ROS
      std::string name1 = "gz"; // this is what appears in the rostopics
      int argc = 0;
      ros::init(argc, NULL, name1);      
    }
    public: ~ROSModelPlugin()
    {
      delete this->nh;
    }

    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      // Store the pointer to the model
      this->model = _parent;

      // ROS Nodehandle
      this->nh = new ros::NodeHandle("~");
      // ROS Subscriber
      this->subvel = this->nh->subscribe<geometry_msgs::Twist>("cmd_vel", 1000, &ROSModelPlugin::ROSCallback_Vel, this );


      this->nh = new ros::NodeHandle("~");
      this->pubpose = this->nh->advertise<geometry_msgs::PoseStamped>("pose",1000);

//***********************************************

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      // for gazebo 1.4 or lower it should be ConnectWorldUpdateStart
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&ROSModelPlugin::OnUpdate, this));
    }

    // Called by the world update start event
    public: void OnUpdate()
    {
      std::vector<double> rangesgz;
        static double qw,qx,qy,qz, Rrad, Prad, Yrad;

// ************* QUATERNION / POSE DATA ******************
      math::Vector3 p = model->GetWorldPose().pos;
      math::Quaternion r = model->GetWorldPose().rot;
      //from quaternion to Roll Pitch Yaw, in radianos
      qw=r.w;   qx=r.x;     qy=r.y;     qz=r.z;
      Rrad=atan2(  2*(qw*qx+qy*qz),  1-2*(qx*qx+qy*qy)  );  //Roll
      Prad=asin(2*(qw*qy-qz*qx));               //Pitch
      Yrad=atan2(  2*(qw*qz+qx*qy),  1-2*(qy*qy+qz*qz)  );  //Yaw

// ***************************************************
        ros::Time current_time, last_time;
        current_time = ros::Time::now();
        last_time = ros::Time::now();

// ******************************************
        //Pub Pose msg
        geometry_msgs::Point p2ros;
        p2ros.x=p.x; p2ros.y=p.y; p2ros.z=p.z;

        geometry_msgs::Quaternion r2ros;
        r2ros.x=r.x; r2ros.y=r.y; r2ros.z=r.z; r2ros.w=r.w;

        geometry_msgs::PoseStamped Pose2ros;
        Pose2ros.header.stamp=scan2ros.header.stamp;//ros::Time::now();
        Pose2ros.header.frame_id="base_link";
        Pose2ros.pose.position=p2ros;
        Pose2ros.pose.orientation=r2ros;

        this->pubpose.publish(Pose2ros);

// *********************************************** 
        //set velocities
           
        this->model->SetLinearVel(math::Vector3(velx, vely, velz));
        this->model->SetAngularVel(math::Vector3(Vangx, Vangy, Vangz));

// ***********************************************
        ros::spinOnce();
    }

    // callback functions run every time data is published to the topic
    void ROSCallback_Vel(const geometry_msgs::Twist::ConstPtr& msg)
    {
        velx=msg->linear.x;
        vely=msg->linear.y;
        velz=msg->linear.z;
        Vangx=msg->angular.x;
        Vangy=msg->angular.y;
        Vangz=msg->angular.z;
    }

    // Pointer to the model
    private: physics::ModelPtr model;


    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    // ROS Nodehandle
    private: ros::NodeHandle* nh;
    // ROS Subscriber
    ros::Subscriber subvel;

    // ROS Publisher
    ros::Publisher pubpose;
    //*************

  };

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(ROSModelPlugin)
}

Originally posted by wilmer on Gazebo Answers with karma: 1 on 2016-12-28

Post score: 0

$\endgroup$

2 Answers 2

0
$\begingroup$

Gazebo Answers logo

Have you installed the geometry messages? For example:

sudo apt-get install ros-kinetic-geometry-msgs

Originally posted by chapulina with karma: 7504 on 2017-01-03

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by wilmer on 2017-01-04:
hi, nop that is not the problem.

I runed and i get this

ros-kinetic-geometry-msgs is already the newest version (1.12.5-0xenial-20161026-181759-0700). ros-kinetic-geometry-msgs set to manually installed. 0 to upgrade, 0 to newly install, 0 to remove and 166 not to upgrade.

can you tell me what are the step to compile a plugin? why i have to add to package.xml

can you check my cmakelist, maybe the errors is there. i added to my initial question

$\endgroup$
0
$\begingroup$

Gazebo Answers logo

I found the error was in package.xml, there was multiple errors.

In theory this plugin should move an object in the enviroment. i had to test the plugin with my robot

thank you for your help chapulina

if someone see an error please write a comment

my plugin at the moment is:

    #include <boost/bind.hpp>
    #include <gazebo/gazebo.hh>
    #include "geometry_msgs/Twist.h"
    #include <gazebo/physics/physics.hh>
    #include <gazebo/common/common.hh>
    #include <stdio.h>
    static float velx=0.0,vely=0.0,velz=0.0,Vangx=0.0,Vangy=0.0, Vangz=0.0; // set as global
    namespace gazebo
    {
      class remusbody : public ModelPlugin
      {
        public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
        {
          // Store the pointer to the model
          this->model = _parent;
          
          // Listen to the update event. This event is broadcast every
          // simulation iteration.
          this->updateConnection = event::Events::ConnectWorldUpdateBegin(
              boost::bind(&remusbody::OnUpdate, this, _1));
        }
        // callback functions run every time data is published to the topic
        void ROSCallback_Vel(const geometry_msgs::Twist::ConstPtr& msg)
        {
            velx=msg->linear.x;
            vely=msg->linear.y;
            velz=msg->linear.z;
            Vangx=msg->angular.x;
            Vangy=msg->angular.y;
            Vangz=msg->angular.z;
        }

        // Called by the world update start event
        public: void OnUpdate(const common::UpdateInfo & /*_info*/)
        {
          // Apply a small linear velocity to the model.
          this->model->SetLinearVel(math::Vector3(velx, vely, velz));
           this->model->SetAngularVel(math::Vector3(Vangx, Vangy, Vangz));
        }
        
        

        // Pointer to the model
        private: physics::ModelPtr model;

        // Pointer to the update event connection
        private: event::ConnectionPtr updateConnection;
      };

      // Register this plugin with the simulator
      GZ_REGISTER_MODEL_PLUGIN(remusbody)
    }

my cmakelist is

    cmake_minimum_required(VERSION 2.8.3)
    project(plugins)

    # Load catkin and all dependencies required for this package
    find_package(catkin REQUIRED COMPONENTS 
      roscpp 
      gazebo_ros
      geometry_msgs 
    )

    # Depend on system install of Gazebo
    set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
    find_package(gazebo REQUIRED)

    link_directories(${GAZEBO_LIBRARY_DIRS})
    include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})

    add_library(plugins SHARED remusbody.cc)
    target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

    catkin_package(
      DEPENDS 
        roscpp 
        gazebo_ros 
        geometry_msgs
    )

and my package.xml inside my plugin folder is

    <?xml version="1.0"?>
    <package>
      <name>plugins</name>
      <version>0.0.0</version>
      <description>The plugins wilmer_gazebo package</description>

      <maintainer email="[email protected]">wilmer</maintainer>



      <license>TODO</license>



      <buildtool_depend>catkin</buildtool_depend>
      <build_depend>roscpp</build_depend>
      <build_depend>rospy</build_depend>
      <build_depend>gazebo_ros</build_depend>
      <build_depend>std_msgs</build_depend>
      <build_depend>geometry_msgs</build_depend>
      <run_depend>roscpp</run_depend>
      <run_depend>rospy</run_depend>
      <run_depend>gazebo_ros</run_depend>
      <run_depend>std_msgs</run_depend>
      <run_depend>geometry_msgs</run_depend>


      <!-- The export tag contains other, unspecified, tags -->
      <export>
        <!-- Other tools can request additional information be placed here -->

      </export>
    </package>

Originally posted by wilmer with karma: 1 on 2017-01-04

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.