initial commit

.gitignore

@@ -0,0 +1,37 @@
+
+# Created by https://www.toptal.com/developers/gitignore/api/ros2
+# Edit at https://www.toptal.com/developers/gitignore?templates=ros2
+
+### ROS2 ###
+install/
+log/
+build/
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+*~
+
+# Emacs
+.#*
+
+# Colcon custom files
+COLCON_IGNORE
+AMENT_IGNORE
+
+# End of https://www.toptal.com/developers/gitignore/api/ros2
+

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "src/Groot"]
+	path = src/Groot
+	url = https://github.com/BehaviorTree/Groot.git

build.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+pushd "$(dirname "$0")"
+colcon build --event-handlers console_cohesion+ --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -G Ninja
+popd

src/Groot

@@ -0,0 +1 @@
+Subproject commit 05fe640172e3cd447ab5db31f71355789f6a48b3

src/btree_nodes/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION VERSION 3.5.1)
+project(btree_nodes)
+
+set(CMAKE_CXX_STANDARD 17)
+add_compile_options(-Wall -Wextra -Wpedantic)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+# find dependencies
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(tree src/Tree.cpp)
+ament_target_dependencies(tree geometry_msgs behaviortree_cpp_v3 rclcpp)
+
+#add_executable(talker src/publisher_member_function.cpp)
+#ament_target_dependencies(talker geometry_msgs rclcpp)
+#add_executable(listener src/subscriber_member_function.cpp)
+#ament_target_dependencies(listener geometry_msgs rclcpp)
+
+install(TARGETS
+  tree
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/btree_nodes/nodes.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<!--
+  For instructions on using Groot and description of the following BehaviorTree nodes,
+  please refer to the groot_instructions.md and REAMDE.md respectively located in the
+  nav2_behavior_tree package.
+-->
+<root>
+    <TreeNodesModel>
+        <!-- ############################### ACTION NODES ################################# -->
+        <Action ID="GenerateNewPose">
+            <input_port name="target_area">Target area</input_port>
+            <output_port name="pose">Generated pose in target area</output_port>
+        </Action>
+
+        <Action ID="MoveToPose">
+            <input_port name="pose_publisher">Where to publish target pose</input_port>
+            <input_port name="pose_listener">Where to get current pose</input_port>
+            <input_port name="target_pose">Actual target pose</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>

src/btree_nodes/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>btree_nodes</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="bastian@todo.todo">bastian</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/btree_nodes/src/Area.cpp

@@ -0,0 +1,27 @@
+//
+// Created by bastian on 21.02.22.
+//
+
+#include "Area.h"
+
+namespace BT
+{
+    template <> inline Area convertFromString(StringView str)
+    {
+        // The next line should be removed...
+        printf("Converting string: \"%s\"\n", str.data() );
+
+        // We expect real numbers separated by semicolons
+        auto parts = splitString(str, ';');
+        if (parts.size() != 2)
+        {
+            throw RuntimeError("invalid input)");
+        }
+        else{
+            Position2d output;
+            output.x     = convertFromString<double>(parts[0]);
+            output.y     = convertFromString<double>(parts[1]);
+            return output;
+        }
+    }
+}

src/btree_nodes/src/Area.h

@@ -0,0 +1,23 @@
+//
+// Created by bastian on 21.02.22.
+//
+
+#ifndef BUILD_AREA_H
+#define BUILD_AREA_H
+
+#include <behaviortree_cpp_v3/behavior_tree.h>
+#include <behaviortree_cpp_v3/bt_factory.h>
+
+class Position2d{
+public:
+    double x;
+    double y;
+};
+
+class Area {
+    int triangleCount;
+    Position2d triangles[0] = {};
+};
+
+
+#endif //BUILD_AREA_H

src/btree_nodes/src/GenerateNewPose.cpp

@@ -0,0 +1,5 @@
+//
+// Created by bastian on 21.02.22.
+//
+
+#include "GenerateNewPose.h"

src/btree_nodes/src/GenerateNewPose.h

@@ -0,0 +1,14 @@
+//
+// Created by bastian on 21.02.22.
+//
+
+#ifndef BUILD_GENERATENEWPOSE_H
+#define BUILD_GENERATENEWPOSE_H
+
+
+class GenerateNewPose {
+
+};
+
+
+#endif //BUILD_GENERATENEWPOSE_H

src/btree_nodes/src/MinimalSubscriber.cpp

@@ -0,0 +1,30 @@
+//
+// Created by bastian on 22.02.22.
+//
+
+#include <memory>
+#include <geometry_msgs/msg/pose.hpp>
+#include "rclcpp/rclcpp.hpp"
+
+template<typename T>
+class MinimalSubscriber : public rclcpp::Node {
+public:
+    MinimalSubscriber(const std::string &node_name, const std::string &topic_name, void(*callback)(T)) :
+            Node(node_name) {
+        this->subscription_ = this->create_subscription<T>(
+                topic_name, 10, [callback](const T result) {
+                    callback(result);
+                });
+    }
+
+private:
+    typename rclcpp::Subscription<T>::SharedPtr subscription_;
+};
+
+template<typename T>
+std::thread spinMinimalSubscriber(const std::string &node_name, const std::string &topic_name, void(*callback)(T)) {
+    return std::thread([node_name, topic_name, callback]() {
+        rclcpp::spin(std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback));
+        rclcpp::shutdown();
+    });
+}

src/btree_nodes/src/Tree.cpp

@@ -0,0 +1,121 @@
+#include <chrono>
+#include "Area.h"
+#include <behaviortree_cpp_v3/action_node.h>
+#include <rclcpp/rclcpp.hpp>
+#include "MinimalSubscriber.cpp"
+
+//-------------------------------------------------------------
+// Simple Action to print a number
+//-------------------------------------------------------------
+
+using namespace BT;
+
+typedef std::chrono::milliseconds Milliseconds;
+
+class MyAsyncAction: public CoroActionNode
+{
+public:
+    MyAsyncAction(const std::string& name):
+            CoroActionNode(name, {})
+    {}
+
+private:
+    // This is the ideal skeleton/template of an async action:
+    //  - A request to a remote service provider.
+    //  - A loop where we check if the reply has been received.
+    //  - You may call setStatusRunningAndYield() to "pause".
+    //  - Code to execute after the reply.
+    //  - A simple way to handle halt().
+    NodeStatus tick() override
+    {
+        std::cout << name() <<": Started. Send Request to server." << std::endl;
+
+        TimePoint initial_time = Now();
+        TimePoint time_before_reply = initial_time + Milliseconds(100);
+
+        int count = 0;
+        bool reply_received = false;
+
+        while( !reply_received )
+        {
+            if( count++ == 0)
+            {
+                // call this only once
+                std::cout << name() <<": Waiting Reply..." << std::endl;
+            }
+            // pretend that we received a reply
+            if( Now() >= time_before_reply )
+            {
+                reply_received = true;
+            }
+
+            if( !reply_received )
+            {
+                // set status to RUNNING and "pause/sleep"
+                // If halt() is called, we will NOT resume execution
+                setStatusRunningAndYield();
+            }
+        }
+
+        // This part of the code is never reached if halt() is invoked,
+        // only if reply_received == true;
+        std::cout << name() <<": Done. 'Waiting Reply' loop repeated "
+                  << count << " times" << std::endl;
+        cleanup(false);
+        return NodeStatus::SUCCESS;
+    }
+
+    // you might want to cleanup differently if it was halted or successful
+    void cleanup(bool halted)
+    {
+        if( halted )
+        {
+            std::cout << name() <<": cleaning up after an halt()\n" << std::endl;
+        }
+        else{
+            std::cout << name() <<": cleaning up after SUCCESS\n" << std::endl;
+        }
+    }
+
+    void halt() override
+    {
+        std::cout << name() <<": Halted." << std::endl;
+        cleanup(true);
+        // Do not forget to call this at the end.
+        CoroActionNode::halt();
+    }
+
+    TimePoint Now()
+    {
+        return std::chrono::high_resolution_clock::now();
+    };
+};
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    /*
+    NodeHandle nh;
+
+    BehaviorTreeFactory factory;
+
+    //factory.registerNodeType<PrintValue>("PrintValue");
+    //RegisterRosService<AddTwoIntsAction>(factory, "AddTwoInts", nh);
+    //RegisterRosAction<FibonacciServer>(factory, "Fibonacci", nh);
+
+    auto tree = factory.createTreeFromText("");
+
+    NodeStatus status = NodeStatus::IDLE;
+
+    while( ros::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING))
+    {
+        ros::spinOnce();
+        status = tree.tickRoot();
+        std::cout << status << std::endl;
+        ros::Duration sleep_time(0.01);
+        sleep_time.sleep();
+    }
+     */
+
+    return 0;
+}

src/cpp_pubsub/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION VERSION 3.5.1)
+project(cpp_pubsub)
+
+set(CMAKE_CXX_STANDARD 17)
+add_compile_options(-Wall -Wextra -Wpedantic)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+# find dependencies
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(talker src/publisher_member_function.cpp)
+ament_target_dependencies(talker geometry_msgs rclcpp)
+add_executable(listener src/subscriber_member_function.cpp)
+ament_target_dependencies(listener geometry_msgs rclcpp)
+
+install(TARGETS
+  talker
+  listener
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/cpp_pubsub/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cpp_pubsub</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="bastian@todo.todo">bastian</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/cpp_pubsub/src/publisher_member_function.cpp

@@ -0,0 +1,54 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// 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 <chrono>
+#include <memory>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+/* This example creates a subclass of Node and uses std::bind() to register a
+ * member function as a callback from the timer. */
+
+class MinimalPublisher : public rclcpp::Node{
+    public:
+        MinimalPublisher(): Node("minimal_publisher") {
+            publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("topic", 10);
+            timer_ = this->create_wall_timer(
+                500ms, [this]{
+                    auto message = geometry_msgs::msg::Pose();
+                    message.position.x=i;
+                    RCLCPP_INFO(this->get_logger(), "Publishing: '%f %f %f'", message.position.x,message.position.y,message.position.z);
+                    i++;
+                    publisher_->publish(message);
+                });
+        }
+
+    private:
+        int i=0;
+        rclcpp::TimerBase::SharedPtr timer_;
+        rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr publisher_;
+};
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<MinimalPublisher>());
+    rclcpp::shutdown();
+    return 0;
+}
+
+#pragma clang diagnostic pop

src/cpp_pubsub/src/subscriber_member_function.cpp

@@ -0,0 +1,66 @@
+// Copyright 2016 Open Source Robotics Foundation, Inc.
+//
+// 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 <memory>
+#include <geometry_msgs/msg/pose.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using std::placeholders::_1;
+/*
+class MinimalSubscriber : public rclcpp::Node
+{
+public:
+  MinimalSubscriber(): Node("minimal_subscriber"){}
+
+private:
+  rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr subscription_ = this->create_subscription<geometry_msgs::msg::Pose>(
+          "topic", 10, [this](const geometry_msgs::msg::Pose PH1) {
+              RCLCPP_INFO(this->get_logger(), "I heard: '%f %f %f'", PH1.position.x,PH1.position.y,PH1.position.z);
+
+              //RCUTILS_LOG_INFO_NAMED(this->get_logger().get_name(),"I heard: '%s'", PH1->data.c_str());
+          });
+};
+*/
+template <typename T>
+class MinimalSubscriber : public rclcpp::Node
+{
+public: MinimalSubscriber(const std::string& node_name, const std::string& topic_name, void(*fun)(T)): Node(node_name){
+        this->subscription_ = this->create_subscription<T>(
+                topic_name, 10, [fun](const T result) {
+                    fun(result);
+                });
+    }
+
+private:
+    typename rclcpp::Subscription<T>::SharedPtr subscription_;
+};
+
+template <typename T>
+std::thread spinMinimalSubscriber(const std::string& node_name, const std::string& topic_name, void(*callback)(T)){
+    return std::thread([node_name,topic_name,callback](){
+        rclcpp::spin(std::make_shared<MinimalSubscriber<T>>(node_name,topic_name,callback));
+        rclcpp::shutdown();
+    });
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(0, nullptr);
+  spinMinimalSubscriber<geometry_msgs::msg::Pose>("talker","topic",[](geometry_msgs::msg::Pose pose){
+      printf("log: %f %f %f \n",pose.position.x,pose.position.y,pose.position.z);
+  }).join();
+  return 0;
+}

src/gaz_simulation/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(gaz_simulation)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(DIRECTORY   launch    DESTINATION   share/${PROJECT_NAME})
+install(DIRECTORY   world     DESTINATION   share/${PROJECT_NAME})
+install(DIRECTORY   urdf      DESTINATION   share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/gaz_simulation/calc.lua

@@ -0,0 +1,16 @@
+#!/usr/bin/lua
+
+io.write("Radius: ")
+local radius=io.read("*line")
+io.write("Height: ")
+local height=io.read("*line")
+io.write("Weight: ")
+local weight=io.read("*line")
+
+local ix_and_y=(1/12)*weight*(3*radius*radius+height*height)
+local iz=(1/2)*weight*radius*radius
+
+print([[<inertial>
+    <mass value="]]..tostring(weight)..[["/>
+    <inertia ixx="]]..tostring(ix_and_y)..[[" ixy="0.0" ixz="0.0" iyy="]]..tostring(ix_and_y)..[[" iyz="0.0" izz="]]..tostring(iz)..[["/>
+</inertial>]])

src/gaz_simulation/launch/test_launch.py

@@ -0,0 +1,72 @@
+import os
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess, IncludeLaunchDescription
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import Command
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    robot_name = "iisy"
+    package_name = "gaz_simulation"
+    pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
+    path = FindPackageShare(package=package_name).find(package_name)
+    world_path = os.path.join(path, "world/test2.xml")
+    urdf_path = os.path.join(path, "urdf/iisy.urdf")
+    return LaunchDescription([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
+            launch_arguments={'world': world_path}.items()
+        ),
+
+        # Start Gazebo client
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
+        ),
+        Node(
+            package='tf2_ros',
+            namespace='gaz_simulation',
+            executable='static_transform_publisher',
+            name='lidar_1_broadcaster',
+            arguments=["1", "-1.9", "1", "1.5707963267949", "0", "0", "map", "lidar_1_link"]
+        ),
+        Node(
+            package='tf2_ros',
+            namespace='gaz_simulation',
+            executable='static_transform_publisher',
+            name='lidar_2_broadcaster',
+            arguments=["-1.9", "1", "1", "0", "0", "0", "map", "lidar_2_link"]
+        ),
+        Node(
+            package='gazebo_ros',
+            executable='spawn_entity.py',
+            arguments=['-entity', robot_name,
+                       '-topic', 'robot_description',
+                       '-x', '-1',
+                       '-y', '-1',
+                       '-z', '1',
+                       '-Y', '0'],  # Yaw
+            output='screen'
+        ),
+        # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            parameters=[{'robot_description': Command(['xacro ', urdf_path])}]
+        ),
+
+        # Publish the joint states of the robot
+        Node(
+            package='joint_state_publisher',
+            executable='joint_state_publisher',
+            name='joint_state_publisher'
+        )
+
+        # ,
+        # Node(
+        #    namespace='turtlesim1',
+        #    executable='gazebo',
+        #    name='gazebo',
+        #    arguments=["test.xml"]
+        # )
+    ])

src/gaz_simulation/package.xml

@@ -0,0 +1,28 @@
+<package format="3">
+  <name>gaz_simulation</name>
+  <version>0.244.1</version>
+  <description>Launch file for simulation of pick and place</description>
+  <license>Apache 2.0</license>
+  <maintainer email="bastian.hofmann@xitaso.com">Bastian Hofmann</maintainer>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>gazebo-11</exec_depend>
+
+  <exec_depend>image_transport_plugins</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros_ign_bridge</exec_depend>
+  <exec_depend>ros_ign_gazebo</exec_depend>
+  <exec_depend>ros_ign_image</exec_depend>
+  <!-- See https://github.com/osrf/ros_ign/issues/40 -->
+  <!--exec_depend>ros_ign_point_cloud</exec_depend-->
+  <exec_depend>rqt_image_view</exec_depend>
+  <exec_depend>rqt_plot</exec_depend>
+  <exec_depend>rqt_topic</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/gaz_simulation/urdf/iisy.urdf

@@ -0,0 +1,195 @@
+<?xml version="1.0"?>
+<robot name="iisy" xmlns:xacro="http://ros.org/wiki/xacro">
+        <static>true</static>
+        <link name='base'>
+            <pose>0.025 0 0.05 0 0 0</pose>
+            <collision name='collision'>
+                <geometry>
+                    <box size=".15 .1 .1"/>
+                </geometry>
+            </collision>
+            <visual name='visual'>
+                <geometry>
+                    <box size=".15 .1 .1"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <mass value="6"/>
+                <inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
+            </inertial>
+        </link>
+        
+        <link name="base_rot">
+            <pose>0 0 0.155 0 0 0</pose>
+            <collision name="collision">
+                <geometry>
+                    <cylinder radius=".05" length=".11"/>
+                </geometry>
+            </collision>
+            <visual name="visual">
+                <geometry>
+                    <cylinder radius=".05" length=".11"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <mass value="2"/>
+                <inertia ixx="0.0032666666666667" ixy="0.0" ixz="0.0" iyy="0.0032666666666667" iyz="0.0" izz="0.0025"/>
+            </inertial>
+        </link>
+        
+        <link name="link1">
+            <pose>0 0 0.36 0 0 0</pose>
+            <collision name="collision">
+                <geometry>
+                    <cylinder radius=".05" length=".30"/>
+                </geometry>
+            </collision>
+            <visual name="visual">
+                <geometry>
+                    <cylinder radius=".05" length=".30"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <mass value="4"/>
+                <inertia ixx="0.0325" ixy="0.0" ixz="0.0" iyy="0.0325" iyz="0.0" izz="0.005"/>
+            </inertial>
+        </link>
+        
+        <link name="link2_rot">
+            <pose>0 0 0.585 0 0 0</pose>
+            <collision name="collision">
+                <geometry>
+                    <cylinder radius=".05" length=".15"/>
+                </geometry>
+            </collision>
+            <visual name="visual">
+                <geometry>
+                    <cylinder radius=".05" length=".15"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <mass value="2"/>
+                <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
+            </inertial>
+        </link>
+        
+        <link name="link2">
+            <pose>0 0 0.735 0 0 0</pose>
+            <collision name="collision">
+                <geometry>
+                    <cylinder radius=".05" length=".15"/>
+                </geometry>
+            </collision>
+            <visual name="visual">
+                <geometry>
+                    <cylinder radius=".05" length=".15"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <mass value="2"/>
+                <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.0025"/>
+            </inertial>
+        </link>
+        
+        <link name="link3_rot">
+            <pose>0 0 0.86 0 0 0</pose>
+            <collision name="collision">
+                <geometry>
+                    <cylinder radius=".05" length=".10"/>
+                </geometry>
+            </collision>
+            <visual name="visual">
+                <geometry>
+                    <cylinder radius=".05" length=".10"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <mass value="2"/>
+                <inertia ixx="0.0029166666666667" ixy="0.0" ixz="0.0" iyy="0.0029166666666667" iyz="0.0" izz="0.0025"/>
+            </inertial>
+        </link>
+        
+        <link name="link3">
+            <pose>0 0 0.94025 0 0 0</pose>
+            <collision name="collision">
+                <geometry>
+                    <cylinder radius=".05" length=".0605"/>
+                </geometry>
+            </collision>
+            <visual name="visual">
+                <geometry>
+                    <cylinder radius=".05" length=".0605"/>
+                </geometry>
+            </visual>
+            <inertial>
+                <mass value="0.8"/>
+                <inertia ixx="0.00074401666666667" ixy="0.0" ixz="0.0" iyy="0.00074401666666667" iyz="0.0" izz="0.001"/>
+            </inertial>
+        </link>
+        
+        <joint type="continuous" name="base_rot_joint">
+            <pose>0 0 -0.055 0 0 0</pose>
+            <parent link="base"/>
+            <child link="base_rot"/>
+            <axis>
+                <xyz>0 0 1</xyz>
+            </axis>
+            <limit effort="30" velocity="1.7453292519943"/>
+        </joint>
+        
+        <joint type="revolute" name="link1_joint">
+            <pose>0 0 -0.15 0 0 0</pose>
+            <parent link="base_rot"/>
+            <child link="link1"/>
+            <axis>
+                <xyz>1 0 0</xyz>
+            </axis>
+            <limit effort="30" velocity="1.7453292519943" lower="-1.221730476396" upper="1.221730476396"/>
+        </joint>
+        
+        <joint type="revolute" name="link2_rot_joint">
+            <pose>0 0 -0.075 0 0 0</pose>
+            <parent link="link1"/>
+            <child link="link2_rot"/>
+            <axis>
+                <xyz>1 0 0</xyz>
+            </axis>
+            <limit effort="30" velocity="1.7453292519943" lower="-1.3089969389957" upper="1.3089969389957"/>
+        </joint>
+        
+        <joint type="continuous" name="link2_joint">
+            <pose>0 0 -0.075 0 0 0</pose>
+            <parent link="link2_rot"/>
+            <child link="link2"/>
+            <axis>
+                <xyz>0 0 1</xyz>
+            </axis>
+            <limit effort="30" velocity="1.7453292519943"/>
+        </joint>
+        
+        <joint type="revolute" name="link3_rot_joint">
+            <pose>0 0 -0.05 0 0 0</pose>
+            <parent link="link2"/>
+            <child link="link3_rot"/>
+            <axis>
+                <xyz>1 0 0</xyz>
+            </axis>
+            <limit effort="30" velocity="1.7453292519943" lower="-0.95993108859688" upper="0.95993108859688"/>
+        </joint>
+        
+        <joint type="continuous" name="link3_joint">
+            <pose>0 0 -0.03025 0 0 0</pose>
+            <parent link="link3_rot"/>
+            <child link="link3"/>
+            <axis>
+                <xyz>0 0 1</xyz>
+            </axis>
+            <limit effort="30" velocity="1.7453292519943"/>
+        </joint>
+        
+        <gazebo>
+            <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+                <robotNamespace>/simple_model</robotNamespace>
+            </plugin>
+        </gazebo>
+</robot>

src/gaz_simulation/world/test2.xml

@@ -0,0 +1,320 @@
+<sdf version='1.7'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+    <model name='walls'>
+      <static>1</static>
+      <link name='walls_1'>
+        <self_collide>0</self_collide>
+        <pose>3 8 1.2</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>10.1 .1 2.4</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>10.1 .1 2.4</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <link name='walls_2'>
+        <self_collide>0</self_collide>
+        <pose>3 -2 1.2</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>10.1 .1 2.4</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>10.1 .1 2.4</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <link name='walls_3'>
+        <self_collide>0</self_collide>
+        <pose>8 3 1.2</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>.1 9.9 2.4</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>.1 9.9 2.4</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+      <link name='walls_4'>
+        <self_collide>0</self_collide>
+        <pose>-2 3 1.2</pose>
+        <collision name='collision'>
+          <geometry>
+            <box>
+              <size>.1 9.9 2.4</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name='visual'>
+          <geometry>
+            <box>
+              <size>.1 9.9 2.4</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+    <model name="lidar_1">
+      <static>1</static>
+      <link name="lidar_1_link">
+        <pose>1 -2 1 0 0 1.5707963267949</pose>
+        <!--<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>-->
+        <sensor name="laser" type="gpu_ray">
+          <pose>0.1 0 0 0 0 0</pose>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>270</samples>
+                <resolution>1</resolution>
+                <min_angle>-2.3561944901923</min_angle>
+                <max_angle>2.3561944901923</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.05</min>
+              <max>10</max>
+              <resolution>0.05</resolution>
+            </range>
+          </ray>
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <visualize>true</visualize>
+
+          <plugin name='laser' filename='libgazebo_ros_ray_sensor.so'>
+            <ros>
+              <namespace>/lidar_1</namespace>
+              <argument>--ros-args --remap ~/out:=scan</argument>
+            </ros>
+            <output_type>sensor_msgs/PointCloud2</output_type>
+          </plugin>
+        </sensor>
+      </link>
+    </model>
+    <model name="lidar_2">
+      <static>1</static>
+      <link name="lidar_2_link">
+        <pose>-2 1 1 0 0 </pose>
+        <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>
+        <sensor name="laser" type="gpu_ray">
+          <pose>0.1 0 0 0 0 0</pose>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>270</samples>
+                <resolution>1</resolution>
+                <min_angle>-2.3561944901923</min_angle>
+                <max_angle>2.3561944901923</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.05</min>
+              <max>10</max>
+              <resolution>0.05</resolution>
+            </range>
+          </ray>
+          <always_on>1</always_on>
+          <update_rate>10</update_rate>
+          <visualize>true</visualize>
+
+          <plugin name='laser' filename='libgazebo_ros_ray_sensor.so'>
+            <ros>
+              <namespace>/lidar_2</namespace>
+              <frameName>/lidar_2_link</frameName>
+              <argument>--ros-args --remap ~/out:=scan</argument>
+            </ros>
+            <output_type>sensor_msgs/PointCloud2</output_type>
+          </plugin>
+        </sensor>
+      </link>
+    </model>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <contact>
+              <collide_bitmask>65535</collide_bitmask>
+              <ode/>
+            </contact>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+    </model>
+    <actor name="actor1">
+      <pose>0 1 1.25 0 0 0</pose>
+
+      <skin>
+        <filename>walk.dae</filename>
+        <scale>1.0</scale>
+      </skin>
+      <animation name="walking">
+        <filename>walk.dae</filename>
+        <scale>1.000000</scale>
+        <interpolate_x>true</interpolate_x>
+      </animation>
+      <plugin name="actor1_plugin" filename="/home/bastian/dev_ws/build/gazebo_ros_actor/libRosActorPlugin.so">
+        <target>2 2 1</target>
+        <target_weight>1.15</target_weight>
+        <obstacle_weight>1.8</obstacle_weight>
+        <animation_factor>5.1</animation_factor>
+        <ignore_obstacles>
+          <model>ground_plane</model>
+        </ignore_obstacles>
+      </plugin>
+    </actor>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <wind/>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <state world_name='default'>
+      <sim_time>21 655000000</sim_time>
+      <real_time>21 863005214</real_time>
+      <wall_time>1644846646 70909467</wall_time>
+      <iterations>21655</iterations>
+      <model name='ground_plane'>
+        <pose>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>0 0 0 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>
+      <light name='sun'>
+        <pose>0 0 10 0 -0 0</pose>
+      </light>
+    </state>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>5 -5 2 0 0.275643 2.35619</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+    <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
+          <ros>
+            <namespace>/demo</namespace>
+            <argument>model_states:=model_states_demo</argument>
+          </ros>
+          <update_rate>1.0</update_rate>
+        </plugin>
+  </world>
+</sdf>

src/gazebo_ros_actor/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION VERSION 3.5.1)
+project(gazebo_ros_actor)
+
+set(CMAKE_CXX_STANDARD 11)
+add_compile_options(-Wall -Wextra -Wpedantic)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rospy REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(gazebo REQUIRED)
+
+link_directories(${GAZEBO_LIBRARY_DIRS})
+include_directories(include ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} )
+
+include_directories(
+        include
+)
+
+add_library(RosActorPlugin SHARED src/RosActorPlugin.cc)
+target_link_libraries(RosActorPlugin ${GAZEBO_LIBRARIES})
+ament_target_dependencies(RosActorPlugin geometry_msgs rclcpp)
+
+install(
+        TARGETS
+        RosActorPlugin
+)
+
+ament_package()

src/gazebo_ros_actor/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>gazebo_ros_actor</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="bastian@todo.todo">bastian</maintainer>
+    <license>TODO: License declaration</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <depend>rclcpp</depend>
+    <depend>geometry_msgs</depend>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <exec_depend>ament_index_python</exec_depend>
+    <exec_depend>rviz2</exec_depend>
+    <exec_depend>launch</exec_depend>
+    <exec_depend>launch_ros</exec_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+        <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}"/>
+    </export>
+</package>

src/gazebo_ros_actor/src/PosePublisher.cc

@@ -0,0 +1,32 @@
+//
+// Created by bastian on 21.02.22.
+//
+
+#include <rclcpp/utilities.hpp>
+#include <rclcpp/executors.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+class PosePublisher : public rclcpp::Node{
+public:
+    PosePublisher(): Node("minimal_publisher") {
+        publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("currentPose", 10);
+    }
+    void update(ignition::math::Pose3d newPose){
+        geometry_msgs::msg::Pose pose;
+        pose.position.x=newPose.X();
+        pose.position.y=newPose.Y();
+        pose.position.z=newPose.Z();
+
+        ignition::math::Quaternion<double> quat;
+        quat.Euler(newPose.Roll(),newPose.Pitch(),newPose.Yaw());
+        pose.orientation.x=quat.X();
+        pose.orientation.y=quat.Y();
+        pose.orientation.z=quat.Z();
+        pose.orientation.w=quat.W();
+
+        publisher_->publish(pose);
+    }
+
+private:
+    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr publisher_;
+};

src/gazebo_ros_actor/src/RosActorPlugin.cc

@@ -0,0 +1,214 @@
+/*
+ * Copyright (C) 2016 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 <functional>
+
+#include <ignition/math.hh>
+#include "gazebo/physics/physics.hh"
+#include "RosActorPlugin.hh"
+#include <memory>
+#include <thread>
+#include <rclcpp/utilities.hpp>
+#include <rclcpp/executors.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+
+using namespace gazebo;
+GZ_REGISTER_MODEL_PLUGIN(RosActorPlugin)
+
+#define WALKING_ANIMATION "walking"
+
+/////////////////////////////////////////////////
+
+RosActorPlugin::RosActorPlugin()
+= default;
+
+class PoseSubscriber : public rclcpp::Node{
+    public:
+        explicit PoseSubscriber(RosActorPlugin* plugin): Node("targetPose"){
+            this->plugin = plugin;
+        }
+
+    private:
+        RosActorPlugin* plugin;
+        rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr subscription_ = this->create_subscription<geometry_msgs::msg::Pose>(
+            "actorTarget", 10, [this](const geometry_msgs::msg::Pose PH1) {
+                RCLCPP_INFO(this->get_logger(), "I heard: '%f %f %f'", PH1.position.x,PH1.position.y,PH1.position.z);
+                plugin->target.Set(PH1.position.x,PH1.position.y,PH1.position.z);
+            });
+};
+
+/////////////////////////////////////////////////
+void RosActorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+{
+    this->sdf = _sdf;
+    this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
+    this->world = this->actor->GetWorld();
+
+    this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
+            [this](auto && PH1) { OnUpdate(std::forward<decltype(PH1)>(PH1)); }));
+
+    this->Reset();
+
+    // Read in the target weight
+    if (_sdf->HasElement("target_weight"))
+        this->targetWeight = _sdf->Get<double>("target_weight");
+    else
+        this->targetWeight = 1.15;
+
+    // Read in the obstacle weight
+    if (_sdf->HasElement("obstacle_weight"))
+        this->obstacleWeight = _sdf->Get<double>("obstacle_weight");
+    else
+        this->obstacleWeight = 1.5;
+
+    // Read in the animation factor (applied in the OnUpdate function).
+    if (_sdf->HasElement("animation_factor"))
+        this->animationFactor = _sdf->Get<double>("animation_factor");
+    else
+        this->animationFactor = 4.5;
+
+    // Add our own name to models we should ignore when avoiding obstacles.
+    this->ignoreModels.push_back(this->actor->GetName());
+
+    // Read in the other obstacles to ignore
+    if (_sdf->HasElement("ignore_obstacles"))
+    {
+        sdf::ElementPtr modelElem =
+                _sdf->GetElement("ignore_obstacles")->GetElement("model");
+        while (modelElem)
+        {
+            this->ignoreModels.push_back(modelElem->Get<std::string>());
+            modelElem = modelElem->GetNextElement("model");
+        }
+    }
+
+    // Create a named topic, and subscribe to it.
+
+    std::thread spinThread([this]{
+        rclcpp::spin(std::make_shared<PoseSubscriber>(this));
+        rclcpp::shutdown();
+    });
+    spinThread.detach();
+}
+
+/////////////////////////////////////////////////
+void RosActorPlugin::Reset()
+{
+    this->velocity = 0.8;
+    this->lastUpdate = 0;
+
+    if (this->sdf && this->sdf->HasElement("target"))
+        this->target = this->sdf->Get<ignition::math::Vector3d>("target");
+    else
+        this->target = ignition::math::Vector3d(0, 0, 1.0);
+
+    actor->SetWorldPose(ignition::math::Pose3d(this->target,ignition::math::Quaterniond(1.5707, 0,0)));
+
+    auto skelAnims = this->actor->SkeletonAnimations();
+    if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
+    {
+        gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
+    }
+    else
+    {
+        // Create custom trajectory
+        this->trajectoryInfo.reset(new physics::TrajectoryInfo());
+        this->trajectoryInfo->type = WALKING_ANIMATION;
+        this->trajectoryInfo->duration = 1.0;
+
+        this->actor->SetCustomTrajectory(this->trajectoryInfo);
+    }
+}
+
+/////////////////////////////////////////////////
+void RosActorPlugin::HandleObstacles(ignition::math::Vector3d &_pos)
+{
+    for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
+    {
+        physics::ModelPtr model = this->world->ModelByIndex(i);
+        if (std::find(this->ignoreModels.begin(), this->ignoreModels.end(),
+                      model->GetName()) == this->ignoreModels.end())
+        {
+            ignition::math::Vector3d offset = model->WorldPose().Pos() -
+                                              this->actor->WorldPose().Pos();
+            double modelDist = offset.Length();
+            if (modelDist < 4.0)
+            {
+                double invModelDist = this->obstacleWeight / modelDist;
+                offset.Normalize();
+                offset *= invModelDist;
+                _pos -= offset;
+            }
+        }
+    }
+}
+
+/////////////////////////////////////////////////
+void RosActorPlugin::OnUpdate(const common::UpdateInfo &_info)
+{
+    // Time delta
+    double dt = (_info.simTime - this->lastUpdate).Double();
+
+    ignition::math::Pose3d pose = this->actor->WorldPose();
+    ignition::math::Vector3d pos = this->target - pose.Pos();
+    ignition::math::Vector3d rpy = pose.Rot().Euler();
+
+    double distance = pos.Length();
+
+    if (distance < 0.3)
+    {
+        return;
+    }
+
+    // Normalize the direction vector, and apply the target weight
+    pos = pos.Normalize() * this->targetWeight;
+
+    // Adjust the direction vector by avoiding obstacles
+    this->HandleObstacles(pos);
+
+    // Compute the yaw orientation
+    ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
+    yaw.Normalize();
+
+    // Rotate in place, instead of jumping.
+    if (std::abs(yaw.Radian()) > IGN_DTOR(10))
+    {
+        pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+
+                                                            yaw.Radian()*0.002);
+    }
+    else
+    {
+        pose.Pos() += pos * this->velocity * dt;
+        pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+yaw.Radian());
+    }
+
+    // Make sure the actor stays within bounds
+    //pose.Pos().X(std::max(-3.0, std::min(3.5, pose.Pos().X())));
+    //pose.Pos().Y(std::max(-10.0, std::min(2.0, pose.Pos().Y())));
+    pose.Pos().Z(1.0);
+
+    // Distance traveled is used to coordinate motion with the walking
+    // animation
+    double distanceTraveled = (pose.Pos() -
+                               this->actor->WorldPose().Pos()).Length();
+
+    publisher.update(pose);
+    this->actor->SetWorldPose(pose, false, false);
+    this->actor->SetScriptTime(this->actor->ScriptTime() +
+                               (distanceTraveled * this->animationFactor));
+    this->lastUpdate = _info.simTime;
+}

src/gazebo_ros_actor/src/RosActorPlugin.hh

@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2016 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.
+ *
+*/
+
+#ifndef GAZEBO_PLUGINS_ACTORPLUGIN_HH_
+#define GAZEBO_PLUGINS_ACTORPLUGIN_HH_
+
+#include <string>
+#include <vector>
+
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/physics/physics.hh"
+#include "gazebo/util/system.hh"
+#include "PosePublisher.cc"
+
+namespace gazebo
+{
+    class GZ_PLUGIN_VISIBLE RosActorPlugin : public ModelPlugin
+{
+    /// \brief Constructor
+    public: RosActorPlugin();
+
+    /// \brief Load the actor plugin.
+    /// \param[in] _model Pointer to the parent model.
+    /// \param[in] _sdf Pointer to the plugin's SDF elements.
+    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+    // Documentation Inherited.
+    public: virtual void Reset();
+
+    private: PosePublisher publisher;
+
+    /// \brief Function that is called every update cycle.
+    /// \param[in] _info Timing information
+    private: void OnUpdate(const common::UpdateInfo &_info);
+
+    /// \brief Helper function to avoid obstacles. This implements a very
+    /// simple vector-field algorithm.
+    /// \param[in] _pos Direction vector that should be adjusted according
+    /// to nearby obstacles.
+    private: void HandleObstacles(ignition::math::Vector3d &_pos);
+
+    /// \brief Pointer to the parent actor.
+    private: physics::ActorPtr actor;
+
+    /// \brief Pointer to the world, for convenience.
+    private: physics::WorldPtr world;
+
+    /// \brief Pointer to the sdf element.
+    private: sdf::ElementPtr sdf;
+
+    /// \brief Velocity of the actor
+    private: ignition::math::Vector3d velocity;
+
+    /// \brief List of connections
+    private: std::vector<event::ConnectionPtr> connections;
+
+    /// \brief Current target location
+    public: ignition::math::Vector3d target;
+
+    /// \brief Target location weight (used for vector field)
+    private: double targetWeight = 1.0;
+
+    /// \brief Obstacle weight (used for vector field)
+    private: double obstacleWeight = 1.0;
+
+    /// \brief Time scaling factor. Used to coordinate translational motion
+    /// with the actor's walking animation.
+    private: double animationFactor = 1.0;
+
+    /// \brief Time of the last update.
+    private: common::Time lastUpdate;
+
+    /// \brief List of models to ignore. Used for vector field
+    private: std::vector<std::string> ignoreModels;
+
+    /// \brief Custom trajectory info.
+    private: physics::TrajectoryInfoPtr trajectoryInfo;
+};
+}
+#endif