yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
Bastian Hofmann 2022-02-25 13:38:01 +01:00
commit c7e1e82b42
29 changed files with 1581 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
.gitignore vendored Normal file
View File

@ -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

3
.gitmodules vendored Normal file
View File

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

4
build.sh Executable file
View File

@ -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

1
src/Groot Submodule

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

42
src/btree_nodes/CMakeLists.txt Normal file
View File

@ -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()

21
src/btree_nodes/nodes.xml Normal file
View File

@ -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>

19
src/btree_nodes/package.xml Normal file
View File

@ -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>

27
src/btree_nodes/src/Area.cpp Normal file
View File

@ -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;
}
}
}

23
src/btree_nodes/src/Area.h Normal file
View File

@ -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

5
src/btree_nodes/src/GenerateNewPose.cpp Normal file
View File

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

14
src/btree_nodes/src/GenerateNewPose.h Normal file
View File

@ -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

30
src/btree_nodes/src/MinimalSubscriber.cpp Normal file
View File

@ -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();
});
}

121
src/btree_nodes/src/Tree.cpp Normal file
View File

@ -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;
}

38
src/cpp_pubsub/CMakeLists.txt Normal file
View File

@ -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()

19
src/cpp_pubsub/package.xml Normal file
View File

@ -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>

54
src/cpp_pubsub/src/publisher_member_function.cpp Normal file
View File

@ -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

66
src/cpp_pubsub/src/subscriber_member_function.cpp Normal file
View File

@ -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;
}

BIN
src/gaz_simulation/.package.xml.kate-swp Normal file
View File

Binary file not shown.

29
src/gaz_simulation/CMakeLists.txt Normal file
View File

@ -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()

16
src/gaz_simulation/calc.lua Executable file
View File

@ -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>]])

72
src/gaz_simulation/launch/test_launch.py Normal file
View File

@ -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"]
# )
])

28
src/gaz_simulation/package.xml Normal file
View File

@ -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>

195
src/gaz_simulation/urdf/iisy.urdf Normal file
View File

@ -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>

320
src/gaz_simulation/world/test2.xml Normal file
View File

@ -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>

30
src/gazebo_ros_actor/CMakeLists.txt Normal file
View File

@ -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()

27
src/gazebo_ros_actor/package.xml Normal file
View File

@ -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>

32
src/gazebo_ros_actor/src/PosePublisher.cc Normal file
View File

@ -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_;
};

214
src/gazebo_ros_actor/src/RosActorPlugin.cc Normal file
View File

@ -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;
}

94
src/gazebo_ros_actor/src/RosActorPlugin.hh Normal file
View File

@ -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