Commit 96f93950 authored by Ibrahim Ahmed's avatar Ibrahim Ahmed
Browse files

first iteration of velocity sensor;

MATLAB does not read velocity readings - perhaps their Gazebo plugin is hardcoded only to listen to a subset of sensors
parent bbf907cf
{
"cmake.configureOnOpen": false
}
\ No newline at end of file
......@@ -7,5 +7,74 @@
"path": "../../src/GazeboPlugin"
}
],
"settings": {},
"settings": {
"files.associations": {
"*.world": "xml",
"*.sdf": "xml",
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"array": "cpp",
"atomic": "cpp",
"strstream": "cpp",
"*.tcc": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"codecvt": "cpp",
"complex": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"list": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"map": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"set": "cpp",
"string": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"fstream": "cpp",
"future": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"ostream": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp"
}
},
}
\ No newline at end of file
<?xml version="1.0"?>
<model>
<name>Octocopter</name>
<version>1.0</version>
<sdf version='1.5'>octocopter.sdf</sdf>
<author>
<name>Ibrahim Ahmed</name>
<email>ibrahim.ahmed@vanderbilt.edu</email>
</author>
<description>
Simplified octocopter model.
</description>
</model>
\ No newline at end of file
<?xml version="1.0"?>
<sdf version="1.5">
<model name="octocopter">
<pose>0 0 0.5 0 0 0</pose>
<static>false</static>
<link name="wholebody">
<inertial>
<mass>1.38</mass>
<inertia>
<ixx>0.114</ixx>
<iyy>0.119</iyy>
<izz>0.218</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.45 0.45 0.27</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>0</mu>
<mu2>0</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.45 0.45 0.27</size>
</box>
</geometry>
</visual>
<sensor name="octocopter-imu" type="imu">
<always_on>1</always_on>
<pose>0 0 0 0 0 0</pose>
<imu>
<angular_velocity>
<x><noise type="none"></noise></x>
<y><noise type="none"></noise></y>
<z><noise type="none"></noise></z>
</angular_velocity>
<linear_acceleration>
<x><noise type="none"></noise></x>
<y><noise type="none"></noise></y>
<z><noise type="none"></noise></z>
</linear_acceleration>
</imu>
</sensor>
</link>
<plugin name="VelocitySensor" filename="libvelocity_sensor.so">
<linkName>wholebody</linkName>
</plugin>
</model>
</sdf>
\ No newline at end of file
cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
add_library(velocity_sensor SHARED velocity_sensor.cc)
target_link_libraries(velocity_sensor ${GAZEBO_LIBRARIES})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
\ No newline at end of file
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/transport.hh>
namespace gazebo
{
class VelocitySensor : public ModelPlugin
{
private:
physics::WorldPtr world;
physics::ModelPtr model;
physics::LinkPtr link;
event::ConnectionPtr updateConnection;
transport::NodePtr node;
transport::PublisherPtr pub;
public:
VelocitySensor(): ModelPlugin()
{
}
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
this->model = _parent;
this->world = _parent->GetWorld();
this->node.reset(new transport::Node());
this->node->Init();
this->pub = this->node->Advertise<msgs::Vector3d>("~/octocopter/wholebody/velocity");
std::string linkName;
if (!_sdf->HasElement("linkName"))
{
this->link = this->model->GetLink();
linkName = link->GetName();
}
else {
linkName = _sdf->GetElement("linkName")->GetValue()->GetAsString();
this->link = this->model->GetLink(linkName);
}
std::cerr << "Velocity sensor loaded into model:" << this->model->GetScopedName()
<< ", on Element: " << linkName << "\n";
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&VelocitySensor::OnUpdate, this));
}
void OnUpdate()
{
ignition::math::v4::Vector3d vel = this->link->WorldLinearVel();
std::cerr << "Update: " << vel << "\n";
msgs::Vector3d msg;
msgs::Set(&msg, vel);
this->pub->Publish(msg);
}
};
GZ_REGISTER_MODEL_PLUGIN(VelocitySensor)
}
\ No newline at end of file
source /usr/share/gazebo/setup.sh
# https://stackoverflow.com/a/9107028/4591810
SCRIPTPATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)/"
SCRIPTPATH="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
export GAZEBO_MODEL_PATH=${SCRIPTPATH}/models/:~/src/gazebo_models/:${GAZEBO_MODEL_PATH}
export GAZEBO_PLUGIN_PATH=~/src/GazeboPlugin/export/lib/:${GAZEBO_PLUGIN_PATH}
\ No newline at end of file
plugin_dirs=""
for plugin_dir in $SCRIPTPATH/plugins/*/build/
do
plugin_dirs=$plugin_dirs:$plugin_dir
done
export GAZEBO_PLUGIN_PATH=~/src/GazeboPlugin/export/lib/:${plugin_dirs}:${GAZEBO_PLUGIN_PATH}
export GAZEBO_MODEL_PATH=${SCRIPTPATH}/models/:~/src/gazebo_models/:${GAZEBO_MODEL_PATH}
\ No newline at end of file
......@@ -87,18 +87,8 @@
<state world_name='default'>
<sim_time>0 0</sim_time>
<real_time>0 0</real_time>
<wall_time>1595535602 92402342</wall_time>
<wall_time>1597304384 766304884</wall_time>
<iterations>0</iterations>
<model name='box'>
<pose frame=''>0.141198 -0.126775 0.5 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0.141198 -0.126775 0.5 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-10 -10 -3.74987 0 -0 0</acceleration>
<wrench>-10 -10 -3.74987 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
......@@ -109,14 +99,24 @@
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='octocopter'>
<pose frame=''>0.002846 0.00316 0.5 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='wholebody'>
<pose frame=''>0.002846 0.00316 0.5 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 -0 0 -0 0</acceleration>
<wrench>0 0 -0 0 -0 0</wrench>
</link>
</model>
<model name='quadrotor'>
<pose frame=''>-0.908023 -2.26733 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>-0.908023 -2.26733 0.182466 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>5.94883 -2.10183 1.00698 -2.48923 0.793794 1.07665</acceleration>
<wrench>7.82866 -2.766 1.32518 0 -0 0</wrench>
<acceleration>0.978164 0.759299 -7.82765 -0.71783 -0.933378 -2.93884</acceleration>
<wrench>1.28726 0.999238 -10.3012 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
......@@ -125,110 +125,144 @@
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>8.65722 -16.059 8.3735 -0 0.371643 2.02419</pose>
<pose frame=''>4.00948 -4.32702 3.63287 0 0.539644 2.25218</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
<plugin name='GazeboPlugin' filename='libGazeboCoSimPlugin.so'>
<portNumber>14581</portNumber>
</plugin>
<model name='box'>
<pose frame=''>0.141198 -0.126775 0.5 0 -0 0</pose>
<static>0</static>
<model name='quadrotor'>
<link name='link'>
<pose frame=''>0 0 0.182466 0 -0 0</pose>
<inertial>
<mass>1</mass>
<mass>1.316</mass>
<inertia>
<ixx>1</ixx>
<ixx>0.0128</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyy>0.0128</iyy>
<iyz>0</iyz>
<izz>1</izz>
<izz>0.0218</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>1 1 1</size>
</box>
<mesh>
<uri>model://quadrotor/meshes/quadrotor_base.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<ode>
<mu>0</mu>
<mu2>0</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</friction>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>1 1 1</size>
</box>
<mesh>
<uri>model://quadrotor/meshes/quadrotor_base.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose frame=''>-0.908023 -2.26733 0 0 -0 0</pose>
</model>
<model name='quadrotor'>
<link name='link'>
<pose frame=''>0 0 0.182466 0 -0 0</pose>
<plugin name='GazeboPlugin' filename='libGazeboCoSimPlugin.so'>
<portNumber>14581</portNumber>
</plugin>
<plugin name='HelloWorld' filename='libhello_world.so'/>
<model name='octocopter'>
<pose frame=''>0.002846 0.00316 0.5 0 -0 0</pose>
<static>0</static>
<link name='wholebody'>
<inertial>
<mass>1.316</mass>
<mass>1.38</mass>
<inertia>
<ixx>0.0128</ixx>
<ixx>0.114</ixx>
<iyy>0.119</iyy>
<izz>0.218</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0128</iyy>
<iyz>0</iyz>
<izz>0.0218</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://quadrotor/meshes/quadrotor_base.dae</uri>
</mesh>
<box>
<size>0.45 0.45 0.27</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<ode>
<mu>0</mu>
<mu2>0</mu2>
</ode>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
<contact>
<ode/>
</contact>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://quadrotor/meshes/quadrotor_base.dae</uri>
</mesh>
<box>
<size>0.45 0.45 0.27</size>
</box>
</geometry>
</visual>
<sensor name='octocopter-imu' type='imu'>
<always_on>1</always_on>
<pose frame=''>0 0 0 0 -0 0</pose>
<imu>
<angular_velocity>
<x>
<noise type='none'/>
</x>
<y>
<noise type='none'/>
</y>
<z>
<noise type='none'/>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type='none'/>
</x>
<y>
<noise type='none'/>
</y>
<z>
<noise type='none'/>
</z>
</linear_acceleration>
<angular_velocity/>
<linear_acceleration/>
</imu>
</sensor>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose frame=''>-0.908023 -2.26733 0 0 -0 0</pose>
<plugin name='VelocitySensor' filename='libvelocity_sensor.so'>
<update_rate>1</update_rate>
</plugin>
</model>
</world>
</sdf>
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment