From 156af3db6bd7c039f0988467f8d8fcc804f76cbe Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Thu, 9 Mar 2023 17:04:51 +0000 Subject: [PATCH] Code cleanup and shutdown fixes --- src/btree_nodes/src/Tree.cpp | 60 ++--- src/btree_nodes/src/nodes/MoveConnection.cpp | 19 +- src/gazebo_ros_actor/CMakeLists.txt | 29 --- src/gazebo_ros_actor/package.xml | 27 --- src/gazebo_ros_actor/src/PosePublisher.cc | 32 --- src/gazebo_ros_actor/src/RosActorPlugin.cc | 217 ------------------- src/gazebo_ros_actor/src/RosActorPlugin.hh | 94 -------- src/gz_ros2_control | 1 + src/ign_actor_plugin/CMakeLists.txt | 14 +- src/ign_actor_plugin/package.xml | 5 +- src/ign_actor_plugin/src/ActorSystem.cpp | 1 - src/ign_actor_plugin/src/ActorSystem.hpp | 9 - 12 files changed, 38 insertions(+), 470 deletions(-) delete mode 100644 src/gazebo_ros_actor/CMakeLists.txt delete mode 100644 src/gazebo_ros_actor/package.xml delete mode 100644 src/gazebo_ros_actor/src/PosePublisher.cc delete mode 100644 src/gazebo_ros_actor/src/RosActorPlugin.cc delete mode 100644 src/gazebo_ros_actor/src/RosActorPlugin.hh create mode 160000 src/gz_ros2_control diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index 13d7cc4..a0ecf55 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -1,6 +1,7 @@ #include "Area.h" #include #include +#include #include #include "nodes/WeightedRandomNode.h" #include "nodes/AmICalled.h" @@ -26,6 +27,7 @@ int main(int argc, char **argv) { node_options.automatically_declare_parameters_from_overrides(true); auto mainNode = rclcpp::Node::make_shared("tree_general_node", node_options); + auto blackboardVector = std::vector(); rclcpp::executors::SingleThreadedExecutor executor; @@ -108,8 +110,16 @@ int main(int argc, char **argv) { }; executor.add_node(actorAnimationNode); + auto actorMovementNode = rclcpp::Node::make_shared("actorMovementNode",node_options); + NodeBuilder builderActorMovement = [&actorMovementNode, &blackboardVector](const std::string &name, const NodeConfiguration &config) { + return std::make_unique(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboardVector](std::shared_ptr feedback){ + for (auto blackboard : blackboardVector){ + blackboard->set("actorPos", feedback); + } + }); + }; - bool called; + bool called = false; auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{ return called ? NodeStatus::SUCCESS : NodeStatus::FAILURE; }; @@ -131,19 +141,20 @@ int main(int argc, char **argv) { Tree actorTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/actorTree.xml"); Tree robotTree = factory.createTreeFromFile("/home/ros/workspace/src/btree_nodes/robotTree.xml"); - auto trees = {actorTree.rootBlackboard(), robotTree.rootBlackboard()}; + blackboardVector.push_back(actorTree.rootBlackboard()); + blackboardVector.push_back(robotTree.rootBlackboard()); + auto init = std::make_shared(); init->position.x = 6.0; init->position.y = 6.0; - for (const auto &item : trees){ - item->set("safeArea",safeArea); - item->set("warningArea",warningArea); - item->set("unsafeArea",unsafeArea); - item->set("negativeYTable",negativeYTable); - item->set("positiveYTable",positiveYTable); - - item->set("actorPos", init); + for (auto blackboard : blackboardVector){ + blackboard->set("safeArea",safeArea); + blackboard->set("warningArea",warningArea); + blackboard->set("unsafeArea",unsafeArea); + blackboard->set("negativeYTable",negativeYTable); + blackboard->set("positiveYTable",positiveYTable); + blackboard->set("actorPos", init); } std::cout << "Starting subscriber." << std::endl; @@ -152,44 +163,21 @@ int main(int argc, char **argv) { while(rclcpp::ok()){ executor.spin(); } + std::cout << "Executor stopped." << std::endl; }).detach(); - - auto actorMovementNode = rclcpp::Node::make_shared("actorMovementNode",node_options); - NodeBuilder builderActorMovement = [&actorMovementNode, &trees](const std::string &name, const NodeConfiguration &config) { - return std::make_unique(name, config, actorMovementNode, "/ActorPlugin/movement",[&trees](std::shared_ptr feedback){ - for (const auto &item : trees){ - item->set("actorPos", feedback); - } - }); - }; - executor.add_node(actorMovementNode); - - MinimalSubscriber::createAsThread("tree_get_currentPose", "currentPose", - [&trees](geometry_msgs::msg::Pose pose) { - auto sharedPose = std::make_shared(pose); - for (const auto &item : trees){ - item->set("actorPos", sharedPose); - } - }).detach(); - std::cout << "Looping." << std::endl; - /*std::thread actor([&actorTree]() { - while (rclcpp::ok()) { - actorTree.tickRoot(); - std::this_thread::sleep_for(std::chrono::milliseconds(20)); - } - });*/ - while (rclcpp::ok()) { robotTree.tickRoot(); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } std::cout << "End." << std::endl; + + rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp index eb076f6..dc1c0a2 100644 --- a/src/btree_nodes/src/nodes/MoveConnection.cpp +++ b/src/btree_nodes/src/nodes/MoveConnection.cpp @@ -30,18 +30,12 @@ void MoveConnection::planAndExecute(const std::shared_ptr &node, const } void MoveConnection::stop() { - std::cout<<"Stopping"< - - - 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 deleted file mode 100644 index 3695adc..0000000 --- a/src/gazebo_ros_actor/src/PosePublisher.cc +++ /dev/null @@ -1,32 +0,0 @@ -// -// Created by bastian on 21.02.22. -// - -#include -#include -#include - -class PosePublisher : public rclcpp::Node{ -public: - PosePublisher(): Node("gazebo_actor_current") { - publisher_ = this->create_publisher("currentPose",10); - } - void update(const 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 deleted file mode 100644 index cff83fb..0000000 --- a/src/gazebo_ros_actor/src/RosActorPlugin.cc +++ /dev/null @@ -1,217 +0,0 @@ -/* - * 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("gazebo_actor_target"){ - this->plugin = plugin; - } - - private: - RosActorPlugin* plugin; - rclcpp::Subscription::SharedPtr subscription_ = this->create_subscription( - "targetPose", 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,1.0); - }); -}; - -///////////////////////////////////////////////// -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) -{ - if(target.X()==HUGE_VAL || target.Y() == HUGE_VAL){ - return; - } - // 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.01) - { - 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 deleted file mode 100644 index 11b48d3..0000000 --- a/src/gazebo_ros_actor/src/RosActorPlugin.hh +++ /dev/null @@ -1,94 +0,0 @@ -/* - * 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 diff --git a/src/gz_ros2_control b/src/gz_ros2_control new file mode 160000 index 0000000..6c4244d --- /dev/null +++ b/src/gz_ros2_control @@ -0,0 +1 @@ +Subproject commit 6c4244de09a53d2352d4db30d7574e8277611a73 diff --git a/src/ign_actor_plugin/CMakeLists.txt b/src/ign_actor_plugin/CMakeLists.txt index 7281ee8..a56af14 100644 --- a/src/ign_actor_plugin/CMakeLists.txt +++ b/src/ign_actor_plugin/CMakeLists.txt @@ -5,13 +5,11 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) project(ign_actor_plugin) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_components REQUIRED) +find_package(ament_cmake REQUIRED) find_package(ignition-cmake2 REQUIRED) find_package(ignition-gazebo6 REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(ros_actor_message_queue_msgs REQUIRED) -find_package(ros_actor_action_server_msgs REQUIRED) find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) @@ -19,22 +17,18 @@ set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) ament_export_dependencies(ign_actor_plugin "rosidl_default_runtime" "ros_actor_action_server_msgs" - "rclcpp" - "rclcpp_action" - "rclcpp_components" + "geometry_msgs" ) # Add sources for each plugin to be registered. add_library(ign_actor_plugin SHARED src/ActorSystem.cpp) -ament_target_dependencies(ign_actor_plugin rclcpp rclcpp_action ros_actor_message_queue_msgs ros_actor_action_server_msgs) +ament_target_dependencies(ign_actor_plugin ros_actor_message_queue_msgs geometry_msgs) set_property(TARGET ign_actor_plugin PROPERTY CXX_STANDARD 17) target_compile_options(ign_actor_plugin PRIVATE -std=c++17) target_link_libraries(ign_actor_plugin ignition-gazebo6::ignition-gazebo6 ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} - ${rclcpp_LIBRARIES} - ${rclcpp_action_LIBRARIES} ) install(TARGETS diff --git a/src/ign_actor_plugin/package.xml b/src/ign_actor_plugin/package.xml index e9c0dfb..82e27f5 100644 --- a/src/ign_actor_plugin/package.xml +++ b/src/ign_actor_plugin/package.xml @@ -8,11 +8,8 @@ TODO: License declaration ament_cmake - rclcpp - rclcpp_action - rclcpp_components ros_actor_message_queue_msgs - ros_actor_action_server_msgs + geometry_msgs ignition-cmake2 ignition-gazebo6 ament_lint_auto diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp index 9c4abc5..8a040a8 100644 --- a/src/ign_actor_plugin/src/ActorSystem.cpp +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -8,7 +8,6 @@ IGNITION_ADD_PLUGIN(ignition::gazebo::ActorSystem, ignition::gazebo::System, ign ignition::gazebo::ActorSystem::ISystemConfigure) using namespace ignition::gazebo; -using namespace ros_actor_action_server_msgs; ActorSystem::ActorSystem() = default; diff --git a/src/ign_actor_plugin/src/ActorSystem.hpp b/src/ign_actor_plugin/src/ActorSystem.hpp index d1ca96e..091d1f0 100644 --- a/src/ign_actor_plugin/src/ActorSystem.hpp +++ b/src/ign_actor_plugin/src/ActorSystem.hpp @@ -36,18 +36,9 @@ #include #include -#include -#include -#include -#include -#include -#include -#include #include using namespace ros_actor_message_queue_msgs; -using namespace ros_actor_action_server_msgs; -using rclcpp_action::ServerGoalHandle; #define AnimationActionServer rclcpp_action::Server #define MovementActionServer rclcpp_action::Server