commit c7e1e82b42e0a2ffaf3a64b3a8326796fe98d6c8 Author: Bastian Hofmann Date: Fri Feb 25 13:38:01 2022 +0100 initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c32553f --- /dev/null +++ b/.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 + diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..cd4da51 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "src/Groot"] + path = src/Groot + url = https://github.com/BehaviorTree/Groot.git diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..a669b9f --- /dev/null +++ b/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 diff --git a/src/Groot b/src/Groot new file mode 160000 index 0000000..05fe640 --- /dev/null +++ b/src/Groot @@ -0,0 +1 @@ +Subproject commit 05fe640172e3cd447ab5db31f71355789f6a48b3 diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt new file mode 100644 index 0000000..d8d6c62 --- /dev/null +++ b/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( 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() diff --git a/src/btree_nodes/nodes.xml b/src/btree_nodes/nodes.xml new file mode 100644 index 0000000..8af5bc3 --- /dev/null +++ b/src/btree_nodes/nodes.xml @@ -0,0 +1,21 @@ + + + + + + + Target area + Generated pose in target area + + + + Where to publish target pose + Where to get current pose + Actual target pose + + + diff --git a/src/btree_nodes/package.xml b/src/btree_nodes/package.xml new file mode 100644 index 0000000..a703b55 --- /dev/null +++ b/src/btree_nodes/package.xml @@ -0,0 +1,19 @@ + + + + btree_nodes + 0.0.0 + TODO: Package description + bastian + TODO: License declaration + + ament_cmake + rclcpp + geometry_msgs + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/btree_nodes/src/Area.cpp b/src/btree_nodes/src/Area.cpp new file mode 100644 index 0000000..af72ef9 --- /dev/null +++ b/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(parts[0]); + output.y = convertFromString(parts[1]); + return output; + } + } +} \ No newline at end of file diff --git a/src/btree_nodes/src/Area.h b/src/btree_nodes/src/Area.h new file mode 100644 index 0000000..fadffec --- /dev/null +++ b/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 +#include + +class Position2d{ +public: + double x; + double y; +}; + +class Area { + int triangleCount; + Position2d triangles[0] = {}; +}; + + +#endif //BUILD_AREA_H diff --git a/src/btree_nodes/src/GenerateNewPose.cpp b/src/btree_nodes/src/GenerateNewPose.cpp new file mode 100644 index 0000000..f59981d --- /dev/null +++ b/src/btree_nodes/src/GenerateNewPose.cpp @@ -0,0 +1,5 @@ +// +// Created by bastian on 21.02.22. +// + +#include "GenerateNewPose.h" diff --git a/src/btree_nodes/src/GenerateNewPose.h b/src/btree_nodes/src/GenerateNewPose.h new file mode 100644 index 0000000..318d945 --- /dev/null +++ b/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 diff --git a/src/btree_nodes/src/MinimalSubscriber.cpp b/src/btree_nodes/src/MinimalSubscriber.cpp new file mode 100644 index 0000000..6042adf --- /dev/null +++ b/src/btree_nodes/src/MinimalSubscriber.cpp @@ -0,0 +1,30 @@ +// +// Created by bastian on 22.02.22. +// + +#include +#include +#include "rclcpp/rclcpp.hpp" + +template +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( + topic_name, 10, [callback](const T result) { + callback(result); + }); + } + +private: + typename rclcpp::Subscription::SharedPtr subscription_; +}; + +template +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>(node_name, topic_name, callback)); + rclcpp::shutdown(); + }); +} \ No newline at end of file diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp new file mode 100644 index 0000000..bb7d0b8 --- /dev/null +++ b/src/btree_nodes/src/Tree.cpp @@ -0,0 +1,121 @@ +#include +#include "Area.h" +#include +#include +#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"); + //RegisterRosService(factory, "AddTwoInts", nh); + //RegisterRosAction(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; +} \ No newline at end of file diff --git a/src/cpp_pubsub/CMakeLists.txt b/src/cpp_pubsub/CMakeLists.txt new file mode 100644 index 0000000..3a77c56 --- /dev/null +++ b/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( 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() diff --git a/src/cpp_pubsub/package.xml b/src/cpp_pubsub/package.xml new file mode 100644 index 0000000..44bd1f8 --- /dev/null +++ b/src/cpp_pubsub/package.xml @@ -0,0 +1,19 @@ + + + + cpp_pubsub + 0.0.0 + TODO: Package description + bastian + TODO: License declaration + + ament_cmake + rclcpp + geometry_msgs + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/cpp_pubsub/src/publisher_member_function.cpp b/src/cpp_pubsub/src/publisher_member_function.cpp new file mode 100644 index 0000000..77318b7 --- /dev/null +++ b/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 +#include +#include + +#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("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::SharedPtr publisher_; +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + +#pragma clang diagnostic pop \ No newline at end of file diff --git a/src/cpp_pubsub/src/subscriber_member_function.cpp b/src/cpp_pubsub/src/subscriber_member_function.cpp new file mode 100644 index 0000000..6a0b945 --- /dev/null +++ b/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 +#include + +#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::SharedPtr subscription_ = this->create_subscription( + "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 +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( + topic_name, 10, [fun](const T result) { + fun(result); + }); + } + +private: + typename rclcpp::Subscription::SharedPtr subscription_; +}; + +template +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>(node_name,topic_name,callback)); + rclcpp::shutdown(); + }); +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(0, nullptr); + spinMinimalSubscriber("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; +} \ No newline at end of file diff --git a/src/gaz_simulation/.package.xml.kate-swp b/src/gaz_simulation/.package.xml.kate-swp new file mode 100644 index 0000000..730b551 Binary files /dev/null and b/src/gaz_simulation/.package.xml.kate-swp differ diff --git a/src/gaz_simulation/CMakeLists.txt b/src/gaz_simulation/CMakeLists.txt new file mode 100644 index 0000000..7e51433 --- /dev/null +++ b/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( 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() diff --git a/src/gaz_simulation/calc.lua b/src/gaz_simulation/calc.lua new file mode 100755 index 0000000..68632b1 --- /dev/null +++ b/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([[ + + +]]) diff --git a/src/gaz_simulation/launch/test_launch.py b/src/gaz_simulation/launch/test_launch.py new file mode 100644 index 0000000..faac12b --- /dev/null +++ b/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"] + # ) + ]) diff --git a/src/gaz_simulation/package.xml b/src/gaz_simulation/package.xml new file mode 100644 index 0000000..476547c --- /dev/null +++ b/src/gaz_simulation/package.xml @@ -0,0 +1,28 @@ + + gaz_simulation + 0.244.1 + Launch file for simulation of pick and place + Apache 2.0 + Bastian Hofmann + + ament_cmake + + gazebo-11 + + image_transport_plugins + robot_state_publisher + ros_ign_bridge + ros_ign_gazebo + ros_ign_image + + + rqt_image_view + rqt_plot + rqt_topic + rviz2 + xacro + + + ament_cmake + + diff --git a/src/gaz_simulation/urdf/iisy.urdf b/src/gaz_simulation/urdf/iisy.urdf new file mode 100644 index 0000000..4bca97e --- /dev/null +++ b/src/gaz_simulation/urdf/iisy.urdf @@ -0,0 +1,195 @@ + + + true + + 0.025 0 0.05 0 0 0 + + + + + + + + + + + + + + + + + + 0 0 0.155 0 0 0 + + + + + + + + + + + + + + + + + + 0 0 0.36 0 0 0 + + + + + + + + + + + + + + + + + + 0 0 0.585 0 0 0 + + + + + + + + + + + + + + + + + + 0 0 0.735 0 0 0 + + + + + + + + + + + + + + + + + + 0 0 0.86 0 0 0 + + + + + + + + + + + + + + + + + + 0 0 0.94025 0 0 0 + + + + + + + + + + + + + + + + + + 0 0 -0.055 0 0 0 + + + + 0 0 1 + + + + + + 0 0 -0.15 0 0 0 + + + + 1 0 0 + + + + + + 0 0 -0.075 0 0 0 + + + + 1 0 0 + + + + + + 0 0 -0.075 0 0 0 + + + + 0 0 1 + + + + + + 0 0 -0.05 0 0 0 + + + + 1 0 0 + + + + + + 0 0 -0.03025 0 0 0 + + + + 0 0 1 + + + + + + + /simple_model + + + diff --git a/src/gaz_simulation/world/test2.xml b/src/gaz_simulation/world/test2.xml new file mode 100644 index 0000000..9d47c94 --- /dev/null +++ b/src/gaz_simulation/world/test2.xml @@ -0,0 +1,320 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + 0 + 3 8 1.2 + + + + 10.1 .1 2.4 + + + + + + + 10.1 .1 2.4 + + + + + + 0 + 3 -2 1.2 + + + + 10.1 .1 2.4 + + + + + + + 10.1 .1 2.4 + + + + + + 0 + 8 3 1.2 + + + + .1 9.9 2.4 + + + + + + + .1 9.9 2.4 + + + + + + 0 + -2 3 1.2 + + + + .1 9.9 2.4 + + + + + + + .1 9.9 2.4 + + + + + + + 1 + + 1 -2 1 0 0 1.5707963267949 + + + 0.1 0 0 0 0 0 + + + + 270 + 1 + -2.3561944901923 + 2.3561944901923 + + + + 0.05 + 10 + 0.05 + + + 1 + 10 + true + + + + /lidar_1 + --ros-args --remap ~/out:=scan + + sensor_msgs/PointCloud2 + + + + + + 1 + + -2 1 1 0 0 + + + + .1 .1 .1 + + + + + + + .1 .1 .1 + + + + + 0.1 0 0 0 0 0 + + + + 270 + 1 + -2.3561944901923 + 2.3561944901923 + + + + 0.05 + 10 + 0.05 + + + 1 + 10 + true + + + + /lidar_2 + /lidar_2_link + --ros-args --remap ~/out:=scan + + sensor_msgs/PointCloud2 + + + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + + 0 1 1.25 0 0 0 + + + walk.dae + 1.0 + + + walk.dae + 1.000000 + true + + + 2 2 1 + 1.15 + 1.8 + 5.1 + + ground_plane + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 21 655000000 + 21 863005214 + 1644846646 70909467 + 21655 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 5 -5 2 0 0.275643 2.35619 + orbit + perspective + + + + + /demo + model_states:=model_states_demo + + 1.0 + + + diff --git a/src/gazebo_ros_actor/CMakeLists.txt b/src/gazebo_ros_actor/CMakeLists.txt new file mode 100644 index 0000000..7d15d78 --- /dev/null +++ b/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() diff --git a/src/gazebo_ros_actor/package.xml b/src/gazebo_ros_actor/package.xml new file mode 100644 index 0000000..1364ad7 --- /dev/null +++ b/src/gazebo_ros_actor/package.xml @@ -0,0 +1,27 @@ + + + + gazebo_ros_actor + 0.0.0 + TODO: Package description + bastian + TODO: License declaration + + ament_cmake + + rclcpp + geometry_msgs + + ament_lint_auto + ament_lint_common + + ament_index_python + rviz2 + launch + launch_ros + + + ament_cmake + + + \ No newline at end of file diff --git a/src/gazebo_ros_actor/src/PosePublisher.cc b/src/gazebo_ros_actor/src/PosePublisher.cc new file mode 100644 index 0000000..4d32235 --- /dev/null +++ b/src/gazebo_ros_actor/src/PosePublisher.cc @@ -0,0 +1,32 @@ +// +// Created by bastian on 21.02.22. +// + +#include +#include +#include + +class PosePublisher : public rclcpp::Node{ +public: + PosePublisher(): Node("minimal_publisher") { + publisher_ = this->create_publisher("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 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::SharedPtr publisher_; +}; \ No newline at end of file diff --git a/src/gazebo_ros_actor/src/RosActorPlugin.cc b/src/gazebo_ros_actor/src/RosActorPlugin.cc new file mode 100644 index 0000000..ebeb602 --- /dev/null +++ b/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 + +#include +#include "gazebo/physics/physics.hh" +#include "RosActorPlugin.hh" +#include +#include +#include +#include +#include + +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::SharedPtr subscription_ = this->create_subscription( + "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(_model); + this->world = this->actor->GetWorld(); + + this->connections.push_back(event::Events::ConnectWorldUpdateBegin( + [this](auto && PH1) { OnUpdate(std::forward(PH1)); })); + + this->Reset(); + + // Read in the target weight + if (_sdf->HasElement("target_weight")) + this->targetWeight = _sdf->Get("target_weight"); + else + this->targetWeight = 1.15; + + // Read in the obstacle weight + if (_sdf->HasElement("obstacle_weight")) + this->obstacleWeight = _sdf->Get("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("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()); + modelElem = modelElem->GetNextElement("model"); + } + } + + // Create a named topic, and subscribe to it. + + std::thread spinThread([this]{ + rclcpp::spin(std::make_shared(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("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; +} diff --git a/src/gazebo_ros_actor/src/RosActorPlugin.hh b/src/gazebo_ros_actor/src/RosActorPlugin.hh new file mode 100644 index 0000000..11b48d3 --- /dev/null +++ b/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 +#include + +#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 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 ignoreModels; + + /// \brief Custom trajectory info. + private: physics::TrajectoryInfoPtr trajectoryInfo; +}; +} +#endif \ No newline at end of file