User Tools

Site Tools


neurali:gazebo_box_animato_source

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
neurali:gazebo_box_animato_source [2016/07/17 00:44] – [CMakeLists.txt] profproneurali:gazebo_box_animato_source [2020/06/08 22:20] (current) – external edit 127.0.0.1
Line 1: Line 1:
 +====== Codice sorgente box animato  ======
  
 +Il codice è utilizzato in questo tutorial [[neurali:gazebo box animato]]
 +
 +Notare l'utilizzo di puntatori boost
 +
 +===== animated_box.cc =====
 +
 +<code cpp>
 +/*
 + * Copyright (C) 2012-2014 Open Source Robotics Foundation
 + *
 + * Licensed under the Apache License, Version 2.0 (the "License");
 + * you may not use this file except in compliance with the License.
 + * You may obtain a copy of the License at
 + *
 +     http://www.apache.org/licenses/LICENSE-2.0
 + *
 + * Unless required by applicable law or agreed to in writing, software
 + * distributed under the License is distributed on an "AS IS" BASIS,
 + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 + * See the License for the specific language governing permissions and
 + * limitations under the License.
 + *
 +*/
 +#include <boost/bind.hpp>
 +#include <gazebo/gazebo.hh>
 +#include <gazebo/physics/physics.hh>
 +#include <gazebo/common/common.hh>
 +#include <stdio.h>
 +
 +namespace gazebo
 +{
 +  class AnimatedBox : public ModelPlugin
 +  {
 +    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
 +    {
 +      // Store the pointer to the model
 +      this->model = _parent;
 +
 +        // create the animation
 +        gazebo::common::PoseAnimationPtr anim(
 +              // name the animation "test",
 +              // make it last 10 seconds,
 +              // and set it on a repeat loop
 +              new gazebo::common::PoseAnimation("test", 10.0, true));
 +
 +        gazebo::common::PoseKeyFrame *key;
 +
 +        // set starting location of the box
 +        key = anim->CreateKeyFrame(0);
 +        key->SetTranslation(math::Vector3(0, 0, 0));
 +        key->SetRotation(math::Quaternion(0, 0, 0));
 +
 +        // set waypoint location after 2 seconds
 +        key = anim->CreateKeyFrame(2.0);
 +        key->SetTranslation(math::Vector3(-50, -50, 0));
 +        key->SetRotation(math::Quaternion(0, 0, 1.5707));
 +
 +
 +        key = anim->CreateKeyFrame(4.0);
 +        key->SetTranslation(math::Vector3(10, 20, 0));
 +        key->SetRotation(math::Quaternion(0, 0, 1.5707));
 +
 +
 +        key = anim->CreateKeyFrame(6.0);
 +        key->SetTranslation(math::Vector3(-10, 20, 0));
 +        key->SetRotation(math::Quaternion(0, 0, 1.5707));
 +
 +
 +        key = anim->CreateKeyFrame(8.0);
 +        key->SetTranslation(math::Vector3(10, -20, 0));
 +        key->SetRotation(math::Quaternion(0, 0, 1.5707));
 +
 +        // set final location equal to starting location
 +        key = anim->CreateKeyFrame(10);
 +        key->SetTranslation(math::Vector3(0, 0, 0));
 +        key->SetRotation(math::Quaternion(0, 0, 0));
 +
 +        // set the animation
 +        _parent->SetAnimation(anim);
 +    }
 +
 +    // 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(AnimatedBox)
 +}
 +
 +</code>
 +
 +===== independent_listener.cc =====
 +
 +
 +<code cpp>
 +/*
 + * Copyright (C) 2012-2014 Open Source Robotics Foundation
 + *
 + * Licensed under the Apache License, Version 2.0 (the "License");
 + * you may not use this file except in compliance with the License.
 + * You may obtain a copy of the License at
 + *
 +     http://www.apache.org/licenses/LICENSE-2.0
 + *
 + * Unless required by applicable law or agreed to in writing, software
 + * distributed under the License is distributed on an "AS IS" BASIS,
 + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 + * See the License for the specific language governing permissions and
 + * limitations under the License.
 + *
 +*/
 +
 +#include <gazebo/transport/transport.hh>
 +#include <gazebo/msgs/msgs.hh>
 +#include <gazebo/gazebo.hh>
 +
 +#include <iostream>
 +
 +/////////////////////////////////////////////////
 +// Function is called every time a message is received.
 +void posesStampedCallback(ConstPosesStampedPtr &posesStamped)
 +{
 +  std::cout << posesStamped->DebugString();
 +
 +  ::google::protobuf::int32 sec = posesStamped->time().sec();
 +  ::google::protobuf::int32 nsec = posesStamped->time().nsec();
 +  std::cout << "Read time: sec: " << sec << " nsec: " << nsec << std::endl;
 +
 +  for (int i =0; i < posesStamped->pose_size(); ++i)
 +  {
 +    const ::gazebo::msgs::Pose &pose = posesStamped->pose(i);
 +    std::string name = pose.name();
 +    if (name == std::string("box"))
 +    {
 +      const ::gazebo::msgs::Vector3d &position = pose.position();
 +
 +      double x = position.x();
 +      double y = position.y();
 +      double z = position.z();
 +
 +      std::cout << "Read position: x: " << x
 +          << " y: " << y << " z: " << z << std::endl;
 +    }
 +  }
 +}
 +
 +/////////////////////////////////////////////////
 +int main(int _argc, char **_argv)
 +{
 +  // Load gazebo
 +  gazebo::setupClient(_argc, _argv);
 +
 +  // Create our node for communication
 +  gazebo::transport::NodePtr node(new gazebo::transport::Node());
 +  node->Init();
 +
 +  // Listen to Gazebo pose info topic
 +  gazebo::transport::SubscriberPtr sub =
 +  node->Subscribe("~/pose/info", posesStampedCallback);
 +
 +  // Busy wait loop...replace with your own code as needed.
 +  while (true)
 +    gazebo::common::Time::MSleep(10);
 +
 +  // Make sure to shut everything down.
 +  gazebo::shutdown();
 +}
 +
 +</code>
 +
 +===== integrated_main.cc =====
 +
 +
 +
 +<code cpp>
 +/*
 + * Copyright (C) 2012-2014 Open Source Robotics Foundation
 + *
 + * Licensed under the Apache License, Version 2.0 (the "License");
 + * you may not use this file except in compliance with the License.
 + * You may obtain a copy of the License at
 + *
 +     http://www.apache.org/licenses/LICENSE-2.0
 + *
 + * Unless required by applicable law or agreed to in writing, software
 + * distributed under the License is distributed on an "AS IS" BASIS,
 + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 + * See the License for the specific language governing permissions and
 + * limitations under the License.
 + *
 + */
 +
 +#include <gazebo/transport/transport.hh>
 +#include <gazebo/msgs/msgs.hh>
 +#include <gazebo/gazebo.hh>
 +#include <gazebo/common/common.hh>
 +#include <gazebo/physics/physics.hh>
 +
 +#include <iostream>
 +
 +/////////////////////////////////////////////////
 +// Function is called every time a message is received.
 +void posesStampedCallback(ConstPosesStampedPtr &posesStamped)
 +{
 +  std::cout << posesStamped->DebugString();
 +
 +  ::google::protobuf::int32 sec = posesStamped->time().sec();
 +  ::google::protobuf::int32 nsec = posesStamped->time().nsec();
 +  std::cout << "Read time: sec: " << sec << " nsec: " << nsec << std::endl;
 +
 +  for (int i =0; i < posesStamped->pose_size(); ++i)
 +  {
 +    const ::gazebo::msgs::Pose &pose = posesStamped->pose(i);
 +    std::string name = pose.name();
 +    if (name == std::string("box"))
 +    {
 +      const ::gazebo::msgs::Vector3d &position = pose.position();
 +
 +      double x = position.x();
 +      double y = position.y();
 +      double z = position.z();
 +
 +      std::cout << "Read position: x: " << x
 +          << " y: " << y << " z: " << z << std::endl;
 +    }
 +  }
 +}
 +
 +/////////////////////////////////////////////////
 +int main(int _argc, char **_argv)
 +{
 +  std::string str = "animated_box.world";
 +  if (_argc > 1)
 +  {
 +    str = _argv[1];
 +  }
 +
 +  // load gazebo server
 +  gazebo::setupServer(_argc, _argv);
 +
 +  // Load a world
 +  gazebo::physics::WorldPtr world = gazebo::loadWorld(str);
 +
 +  // Create our node for communication
 +  gazebo::transport::NodePtr node(new gazebo::transport::Node());
 +  node->Init();
 +
 +  // Listen to Gazebo pose information topic
 +  gazebo::transport::SubscriberPtr sub =
 +  node->Subscribe("~/pose/info", posesStampedCallback);
 +
 +  // Busy wait loop...replace with your own code as needed.
 +  while (true)
 +  {
 +    // Run simulation for 100 steps.
 +    gazebo::runWorld(world, 100);
 +  }
 +
 +  // Make sure to shut everything down.
 +  gazebo::shutdown();
 +}
 +
 +</code>
 +
 +
 +===== CMakeLists.txt =====
 +
 +http://wiki.ros.org/catkin/CMakeLists.txt
 +
 +<code>
 +cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
 +
 +project(animated_box)
 +
 +# Find packages
 +
 +find_package(Boost REQUIRED COMPONENTS system)
 +include_directories(${Boost_INCLUDE_DIRS})
 +link_directories(${Boost_LIBRARY_DIRS})
 +
 +
 +find_package(Protobuf REQUIRED)
 +
 +include (FindPkgConfig)
 +if (PKG_CONFIG_FOUND)
 +  pkg_check_modules(GAZEBO gazebo)
 +endif()
 +
 +
 +# include appropriate directories
 +include_directories(${GAZEBO_INCLUDE_DIRS})
 +link_directories(${GAZEBO_LIBRARY_DIRS})
 +
 +# Create libraries and executables
 +
 +add_library(animated_box SHARED animated_box.cc)
 +target_link_libraries(animated_box ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
 +
 +add_executable(integrated_main integrated_main.cc)
 +target_link_libraries(integrated_main ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
 +
 +
 +add_executable(independent_listener independent_listener.cc)
 +target_link_libraries(independent_listener ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} pthread)
 +</code>
 +
 +
 +===== animated_box.world =====
 +
 +
 +<code xml>
 +<?xml version="1.0"?> 
 +<sdf version="1.4">
 +  <world name="animated_box_world">
 +
 +    <!-- Ground Plane -->
 +    <include>
 +      <uri>model://ground_plane</uri>
 +    </include>
 +
 +    <include>
 +      <uri>model://sun</uri>
 +    </include>
 +
 +    <model name="box">
 +      <pose>0 0 0.5 0 0 0</pose>
 +      <link name="link">
 +        <collision name="collision">
 +          <geometry>
 +            <box>
 +              <size>1 1 1</size>
 +            </box>
 +          </geometry>
 +        </collision>
 +
 +        <visual name="visual">
 +          <geometry>
 +            <box>
 +              <size>1 1 1</size>
 +            </box>
 +          </geometry>
 +        </visual>
 +      </link>
 +
 +      <plugin name="push_animate" filename="libanimated_box.so"/>
 +    </model>        
 +  </world>
 +</sdf>
 +
 +</code>