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

Code cleanup and shutdown fixes

Browse Source
This commit is contained in:
Bastian Hofmann 2023-03-09 17:04:51 +00:00
parent 7293a2b797
commit 156af3db6b
12 changed files with 38 additions and 470 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/btree_nodes/src/Tree.cpp
View File

@ -1,6 +1,7 @@
#include "Area.h"
#include <rclcpp/rclcpp.hpp>
#include <random>
#include <rclcpp/utilities.hpp>
#include <std_msgs/msg/bool.hpp>
#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<BT::Blackboard::Ptr>();
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<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&blackboardVector](std::shared_ptr<const Movement::Feedback> 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<geometry_msgs::msg::Pose>();
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,38 +163,13 @@ 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<ActorMovement>(name, config, actorMovementNode, "/ActorPlugin/movement",[&trees](std::shared_ptr<const Movement::Feedback> feedback){
for (const auto &item : trees){
item->set("actorPos", feedback);
}
});
};
executor.add_node(actorMovementNode);
MinimalSubscriber<geometry_msgs::msg::Pose>::createAsThread("tree_get_currentPose", "currentPose",
[&trees](geometry_msgs::msg::Pose pose) {
auto sharedPose = std::make_shared<geometry_msgs::msg::Pose>(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));
@ -191,5 +177,7 @@ int main(int argc, char **argv) {
std::cout << "End." << std::endl;
rclcpp::shutdown();
return 0;
}

19
src/btree_nodes/src/nodes/MoveConnection.cpp
View File

@ -30,18 +30,12 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
std::cout<<"Planned a path."<<std::endl;
}else{
std::cout<<"Error during planning."<<std::endl;
lock.unlock();
callback(false);
}
if(cancelled){
lastTarget = nullptr;
lock.unlock();
callback(false);
return;
}
bool success = moveGroup.execute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
if(!cancelled) {
callback(success);
}
callback( moveGroup.execute(plan) == moveit::core::MoveItErrorCode::SUCCESS );
lastTarget = nullptr;
lock.unlock();
@ -67,9 +61,12 @@ MoveConnection::MoveConnection(const std::shared_ptr<rclcpp::Node> &node, const
}
void MoveConnection::stop() {
std::cout<<"Stopping"<<std::endl;
std::cout<<"Stopping MoveConnection"<<std::endl;
cancelled = true;
moveGroup.stop();
while (lastTarget != nullptr) {}
if(rclcpp::ok()){
moveGroup.stop();
while (lastTarget != nullptr) {}
}
cancelled = false;
std::cout<<"Stopped MoveConnection"<<std::endl;
}

29
src/gazebo_ros_actor/CMakeLists.txt
View File

@ -1,29 +0,0 @@
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(tf2_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 tf2_geometry_msgs rclcpp)
#install(
# TARGETS
# RosActorPlugin
#)
ament_package()

27
src/gazebo_ros_actor/package.xml
View File

@ -1,27 +0,0 @@
<?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
View File

@ -1,32 +0,0 @@
//
// 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("gazebo_actor_current") {
publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("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<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_;
};

217
src/gazebo_ros_actor/src/RosActorPlugin.cc
View File

@ -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 <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("gazebo_actor_target"){
this->plugin = plugin;
}
private:
RosActorPlugin* plugin;
rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr subscription_ = this->create_subscription<geometry_msgs::msg::Pose>(
"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<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)
{
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;
}

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

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

1
src/gz_ros2_control Submodule

@ -0,0 +1 @@
Subproject commit 6c4244de09a53d2352d4db30d7574e8277611a73

14
src/ign_actor_plugin/CMakeLists.txt
View File

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

5
src/ign_actor_plugin/package.xml
View File

@ -8,11 +8,8 @@
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_components</depend>
<depend>ros_actor_message_queue_msgs</depend>
<depend>ros_actor_action_server_msgs</depend>
<depend>geometry_msgs</depend>
<depend>ignition-cmake2</depend>
<depend>ignition-gazebo6</depend>
<test_depend>ament_lint_auto</test_depend>

1
src/ign_actor_plugin/src/ActorSystem.cpp
View File

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

9
src/ign_actor_plugin/src/ActorSystem.hpp
View File

@ -36,18 +36,9 @@
#include <ignition/gazebo/components/Pose.hh>
#include <sdf/Actor.hh>
#include <rclcpp/node.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/server.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <ros_actor_action_server_msgs/action/animation.hpp>
#include <ros_actor_action_server_msgs/action/movement.hpp>
#include <ros_actor_message_queue_msgs/MessageTypes.hpp>
using namespace ros_actor_message_queue_msgs;
using namespace ros_actor_action_server_msgs;
using rclcpp_action::ServerGoalHandle;
#define AnimationActionServer rclcpp_action::Server<action::Animation>
#define MovementActionServer rclcpp_action::Server<action::Movement>