neurali:gazebo_box_animato_source
Table of Contents
Codice sorgente box animato
Il codice è utilizzato in questo tutorial gazebo box animato
Notare l'utilizzo di puntatori boost
animated_box.cc
/* * 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) }
independent_listener.cc
/* * 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(); }
integrated_main.cc
/* * 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(); }
CMakeLists.txt
http://wiki.ros.org/catkin/CMakeLists.txt
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)
animated_box.world
<?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>
neurali/gazebo_box_animato_source.txt · Last modified: 2020/06/08 22:20 by 127.0.0.1