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

Never changed ActorPlugin to Action, instead minor renaming and tweaks.

Browse Source 
Updated launch file to support GPU-acceleration of the gazebo client.
Created behavior tree for actor
Updated robot urdf, added better visualization and colliders.
This commit is contained in:
Bastian Hofmann 2022-03-14 21:02:55 +01:00
parent 3f9b39f9d6
commit 80da2d4153
28 changed files with 610 additions and 311 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/btree_nodes/CMakeLists.txt
View File

@ -1,7 +1,7 @@
cmake_minimum_required(VERSION VERSION 3.5.1)
project(btree_nodes)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD 20)
add_compile_options(-Wall -Wextra -Wpedantic)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
@ -11,13 +11,15 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_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 src/Extensions.cpp)
ament_target_dependencies(tree geometry_msgs behaviortree_cpp_v3 rclcpp)
add_executable(tree src/Tree.cpp src/Extensions.cpp src/WeightedRandomNode.cpp)
ament_target_dependencies(tree rclcpp geometry_msgs std_msgs behaviortree_cpp_v3)
#add_executable(talker src/publisher_member_function.cpp)
#ament_target_dependencies(talker geometry_msgs rclcpp)

26
src/btree_nodes/nodes.xml
View File

@ -7,16 +7,28 @@
<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 ID="AmICalled">
<input_port name="called">called status</input_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 ID="GenerateSafeTarget">
<output_port name="target">Generated pose in safe area</output_port>
</Action>
<Action ID="GenerateWarningTarget">
<output_port name="target">Generated pose in warning area</output_port>
</Action>
<Action ID="GenerateUnsafeTarget">
<output_port name="target">Generated pose in unsafe area</output_port>
</Action>
<Action ID="MoveActorToTarget">
<input_port name="current">Current actor position</input_port>
<input_port name="target">Target to move actor to</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
<Condition ID="InTest">

9
src/btree_nodes/src/MinimalSubscriber.cpp
View File

@ -9,7 +9,7 @@
template<typename T>
class MinimalSubscriber : public rclcpp::Node {
public:
MinimalSubscriber(const std::string &node_name, const std::string &topic_name, void(*callback)(T)) :
MinimalSubscriber(const std::string &node_name, const std::string &topic_name, std::function<void(T)> callback) :
Node(node_name) {
this->subscription_ = this->create_subscription<T>(
topic_name, 10, [callback](const T result) {
@ -22,9 +22,10 @@ private:
};
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));
std::thread spinMinimalSubscriber(const std::string &node_name, const std::string &topic_name, std::function<void(T)> callback) {
auto subscriber = std::make_shared<MinimalSubscriber<T>>(node_name, topic_name, callback);
return std::thread([subscriber]() {
rclcpp::spin(subscriber);
rclcpp::shutdown();
});
}

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

@ -3,7 +3,9 @@
#include <behaviortree_cpp_v3/action_node.h>
#include <rclcpp/rclcpp.hpp>
#include <random>
#include <std_msgs/msg/bool.hpp>
#include "MinimalSubscriber.cpp"
#include "WeightedRandomNode.h"
//-------------------------------------------------------------
// Simple Action to print a number
@ -11,73 +13,96 @@
using namespace BT;
typedef std::chrono::milliseconds Milliseconds;
class MyAsyncAction : public CoroActionNode {
class AmICalled: public ConditionNode
{
public:
MyAsyncAction(const std::string &name) :
CoroActionNode(name, {}) {}
AmICalled(const std::string& name, const NodeConfiguration& config):
ConditionNode(name,config)
{}
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;
static PortsList providedPorts()
{
return { InputPort<bool>("called") };
}
if (!reply_received) {
// set status to RUNNING and "pause/sleep"
// If halt() is called, we will NOT resume execution
setStatusRunningAndYield();
NodeStatus tick() override
{
bool called;
if(!getInput("called",called)){
return NodeStatus::FAILURE;
}
}
// 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);
if(called){
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;
return NodeStatus::FAILURE;
}
}
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();
};
typedef std::chrono::milliseconds Milliseconds;
class MoveActorToTarget : public StatefulActionNode {
public:
MoveActorToTarget(const std::string &name, const NodeConfiguration &config)
: StatefulActionNode(name, config) {}
static PortsList providedPorts() {
return {
InputPort<geometry_msgs::msg::Pose>("current"),
InputPort<geometry_msgs::msg::Pose>("target")
};
}
geometry_msgs::msg::Pose target;
NodeStatus onStart() override {
std::cout << "started moving" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
auto res = getInput<geometry_msgs::msg::Pose>("target", target);
if (!res) {
std::cout << "[ no target available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
publisher->publish(target);
return NodeStatus::RUNNING;
}
NodeStatus onRunning() override {
geometry_msgs::msg::Pose current;
auto res = getInput<geometry_msgs::msg::Pose>("current", current);
if (!res) {
std::cout << "[ no current position available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
double dX = target.position.x - current.position.x;
double dY = target.position.y - current.position.y;
auto distance = sqrt(dX * dX + dY * dY);
if (distance < 0.3) {
return NodeStatus::SUCCESS;
} else {
return NodeStatus::RUNNING;
}
}
void onHalted() override {
std::cout << "halted move" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
geometry_msgs::msg::Pose inf;
inf.position.x = HUGE_VAL;
inf.position.y = HUGE_VAL;
publisher->publish(inf);
}
};
// This class cannot guarantee valid results with every polygon, triangles are assumed to be a valid triangle-strip.
@ -153,7 +178,7 @@ public:
static PortsList providedPorts() {
// Optionally, a port can have a human readable description
const char *description = "Generates a random position in an area defined by a triangle strip";
return {OutputPort<Position2D>("target", description)};
return {OutputPort<geometry_msgs::msg::Pose>("target", description)};
}
@ -163,57 +188,88 @@ public:
while (triangleRandom < weights[i + 1] && i < weights.size() - 1) {
i++;
}
Position2D randomPos = getRandomPosInTriangle(i);
auto pos = getRandomPosInTriangle(i);
setOutput<Position2D>("targetPosition", randomPos);
geometry_msgs::msg::Pose randomPose;
randomPose.position.x=pos.x;
randomPose.position.y=pos.y;
printf("Generated Target: %f %f\n",pos.x,pos.y);
setOutput<geometry_msgs::msg::Pose>("target", randomPose);
return NodeStatus::SUCCESS;
}
};
int main(int argc, char **argv) {
std::cout << "Registering nodes." << std::endl;
NodeBuilder builderGenerateSafeTarget = [](const std::string &name, const NodeConfiguration &config) {
const Area areaSafe = {std::vector<Position2D>({{1, 1},
const Area area = {std::vector<Position2D>({{1, 1},
{5, 1},
{5, 5},
{1, 5}})};
return std::make_unique<GenerateTarget2D>(name, config, area);
};
NodeBuilder builderGenerateWarningTarget = [](const std::string &name, const NodeConfiguration &config) {
const Area area = {std::vector<Position2D>({{1, 1},
{2, 2},
{1, 2}})};
return std::make_unique<GenerateTarget2D>(name, config, areaSafe);
return std::make_unique<GenerateTarget2D>(name, config, area);
};
NodeBuilder builderGenerateUnsafeTarget = [](const std::string &name, const NodeConfiguration &config) {
const Area area = {std::vector<Position2D>({{1, 1},
{2, 2},
{1, 2}})};
return std::make_unique<GenerateTarget2D>(name, config, area);
};
rclcpp::init(argc, argv);
new Area();
BehaviorTreeFactory factory;
factory.registerBuilder<GenerateTarget2D>("GenerateSafeTarget", builderGenerateSafeTarget);
factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateSafeTarget);
factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateSafeTarget);
factory.registerBuilder<GenerateTarget2D>("GenerateWarningTarget", builderGenerateWarningTarget);
factory.registerBuilder<GenerateTarget2D>("GenerateUnsafeTarget", builderGenerateUnsafeTarget);
Tree tree = factory.createTreeFromFile("actor_tree.xml");
factory.registerNodeType<AmICalled>("AmICalled");
factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
std::cout << "Creating tree." << std::endl;
Tree tree = factory.createTreeFromFile("/home/bastian/dev_ws/src/btree_nodes/tree.xml");
std::mutex mutex;
std::cout << "Starting subscriber." << std::endl;
geometry_msgs::msg::Pose init;
tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("current",init);
tree.rootBlackboard()->set<geometry_msgs::msg::Pose>("target",init);
tree.rootBlackboard()->set<bool>("called",false);
spinMinimalSubscriber<geometry_msgs::msg::Pose>("tree_get_currentPose", "currentPose",
[&tree, &mutex](geometry_msgs::msg::Pose pose) {
mutex.lock();
tree.rootBlackboard()->set("current", pose);
mutex.unlock();
}).detach();
spinMinimalSubscriber<std_msgs::msg::Bool>("tree_get_called", "called",
[&tree, &mutex](std_msgs::msg::Bool called) {
mutex.lock();
tree.rootBlackboard()->set("called", (bool) called.data);
mutex.unlock();
}).detach();
std::cout << "Looping." << std::endl;
while (rclcpp::ok()) {
mutex.lock();
tree.tickRoot();
mutex.unlock();
}
/*
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( rclcpp::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING))
{
rclcpp::spinOnce();
status = tree.tickRoot();
std::cout << status << std::endl;
ros::Duration sleep_time(0.01);
sleep_time.sleep();
}
*/
std::cout << "End." << std::endl;
return 0;
}

70
src/btree_nodes/src/WeightedRandomNode.cpp Normal file
View File

@ -0,0 +1,70 @@
//
// Created by bastian on 08.03.22.
//
#include "WeightedRandomNode.h"
#include <regex>
WeightedRandomNode::WeightedRandomNode(const std::string &name, const NodeConfiguration &config)
: ControlNode::ControlNode(name, config), selected_child_index(0), select_next(true) {
setRegistrationID("Sequence");
}
void WeightedRandomNode::halt() {
select_next = true;
ControlNode::halt();
}
NodeStatus WeightedRandomNode::tick() {
if (select_next) {
const size_t children_count = children_nodes_.size();
std::string weightString;
if(!getInput<std::string>("weights",weightString)){
std::cout<< "Weights are REQUIRED." <<std::endl;
return NodeStatus::FAILURE;
}
std::regex rgx(";");
std::sregex_token_iterator iter(weightString.begin(),
weightString.end(),
rgx,
-1);
std::sregex_token_iterator end;
std::vector<double> weights;
double sum = 0;
unsigned long i=0;
for (; iter != end; ++iter) {
sum+=stod(*iter);
weights.push_back(sum);
i++;
}
if(weights.size()!=children_count){
std::cout<< "Number of weights doesn't match child count." <<std::endl;
return NodeStatus::FAILURE;
}
std::uniform_real_distribution<double> randomRange(0, sum);
double random = randomRange(rng);
i=0;
while (i<weights.size() && random>weights[i]){
i++;
}
selected_child_index = i;
select_next = false;
}
TreeNode *current_child_node = children_nodes_[selected_child_index];
const NodeStatus child_status = current_child_node->executeTick();
if (child_status != NodeStatus::RUNNING) {
select_next = true;
printf("node %zu finished.\n",selected_child_index);
}
return child_status;
}

35
src/btree_nodes/src/WeightedRandomNode.h Normal file
View File

@ -0,0 +1,35 @@
//
// Created by bastian on 08.03.22.
//
#ifndef BUILD_WEIGHTEDRANDOMNODE_H
#define BUILD_WEIGHTEDRANDOMNODE_H
#include <behaviortree_cpp_v3/control_node.h>
#include <string>
#include <random>
using namespace BT;
class WeightedRandomNode : public ControlNode
{
public:
WeightedRandomNode(const std::string& name, const NodeConfiguration &config);
virtual ~WeightedRandomNode() override = default;
virtual void halt() override;
static PortsList providedPorts(){
return { InputPort<bool>("weights") };
}
private:
size_t selected_child_index;
bool select_next{};
std::mt19937_64 rng;
virtual BT::NodeStatus tick() override;
};
#endif //BUILD_WEIGHTEDRANDOMNODE_H

42
src/btree_nodes/tree.xml Normal file
View File

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<ReactiveSequence>
<Action ID="AmICalled" called="{called}"/>
<Sequence>
<Control ID="WeightedRandom" weights="10;2;1">
<Action ID="GenerateSafeTarget" target="{target}"/>
<Action ID="GenerateWarningTarget" target="{target}"/>
<Action ID="GenerateUnsafeTarget" target="{target}"/>
</Control>
<Action ID="MoveActorToTarget" current="{current}" target="{target}"/>
</Sequence>
</ReactiveSequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="AmICalled">
<input_port name="called">called status</input_port>
</Action>
<Action ID="GenerateSafeTarget">
<output_port name="target">Generated pose in safe area</output_port>
</Action>
<Action ID="GenerateUnsafeTarget">
<output_port name="target">Generated pose in unsafe area</output_port>
</Action>
<Action ID="GenerateWarningTarget">
<output_port name="target">Generated pose in warning area</output_port>
</Action>
<Condition ID="InTest"/>
<Action ID="MoveActorToTarget">
<input_port name="current">Current actor position</input_port>
<input_port name="target">Target to move actor to</input_port>
</Action>
<Control ID="WeightedRandom">
<input_port name="weights">Weights for the children, separated by semicolon.</input_port>
</Control>
</TreeNodesModel>
<!-- ////////// -->
</root>

2
src/cpp_pubsub/src/publisher_member_function.cpp
View File

@ -27,7 +27,7 @@ using namespace std::chrono_literals;
class MinimalPublisher : public rclcpp::Node{
public:
MinimalPublisher(): Node("minimal_publisher") {
publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("topic", 10);
publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
timer_ = this->create_wall_timer(
500ms, [this]{
auto message = geometry_msgs::msg::Pose();

1
src/gaz_simulation/CMakeLists.txt
View File

@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED)
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
install(DIRECTORY world DESTINATION share/${PROJECT_NAME})
install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})
install(DIRECTORY stl DESTINATION share/${PROJECT_NAME})
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)

14
src/gaz_simulation/launch/test_launch.py
View File

@ -14,11 +14,19 @@ def generate_launch_description():
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()
#IncludeLaunchDescription(
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
# launch_arguments={'world': world_path}.items()
#),
ExecuteProcess(
cmd=[[
'LIBGL_ALWAYS_SOFTWARE=true ros2 launch gazebo_ros gzserver.launch.py "world:='+world_path+'"'
]],
shell=True
),
# Start Gazebo client
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))

BIN
src/gaz_simulation/stl/base_bottom_coll.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/base_bottom_low.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/base_top_low.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_1_full_coll.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_1_full_low.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_2_bottom_low.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_2_top_coll.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_2_top_low.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_3_bottom_coll.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_3_bottom_low.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_3_top_coll.stl Executable file
View File

Binary file not shown.

BIN
src/gaz_simulation/stl/link_3_top_low.stl Executable file
View File

Binary file not shown.

272
src/gaz_simulation/urdf/iisy.urdf
View File

@ -1,195 +1,263 @@
<?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'>
<robot name="iisy">
<link name="world" />
<link name="base_link"/>
<link name="base">
<collision>
<geometry>
<box size=".15 .1 .1"/>
<mesh filename="file://$(find gaz_simulation)/stl/base_bottom_coll.stl"/>
</geometry>
</collision>
<visual name='visual'>
<visual>
<geometry>
<box size=".15 .1 .1"/>
<mesh filename="file://$(find gaz_simulation)/stl/base_bottom_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0.025 0 0.05" rpy="0 0 0"/>
<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">
</link>
<link name="base_top">
<collision>
<origin xyz="0 -0.005 0.08"/>
<geometry>
<cylinder radius=".05" length=".11"/>
<box size="0.14 0.15 0.16"/>
</geometry>
</collision>
<visual name="visual">
<visual>
<geometry>
<cylinder radius=".05" length=".11"/>
<mesh filename="file://$(find gaz_simulation)/stl/base_top_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.155" rpy="0 0 0"/>
<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">
</link>
<link name="link1_full">
<collision>
<geometry>
<cylinder radius=".05" length=".30"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_1_full_coll.stl"/>
</geometry>
</collision>
<visual name="visual">
<visual>
<geometry>
<cylinder radius=".05" length=".30"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_1_full_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.36" rpy="0 0 0"/>
<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">
</link>
<link name="link2_bottom">
<collision>
<origin xyz="0 0.07 0.025"/>
<geometry>
<cylinder radius=".05" length=".15"/>
<box size="0.14 0.14 0.19"/>
</geometry>
</collision>
<visual name="visual">
<visual>
<geometry>
<cylinder radius=".05" length=".15"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_2_bottom_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.585" rpy="0 0 0"/>
<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">
</link>
<link name="link2_top">
<collision>
<geometry>
<cylinder radius=".05" length=".15"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_2_top_coll.stl"/>
</geometry>
</collision>
<visual name="visual">
<visual>
<geometry>
<cylinder radius=".05" length=".15"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_2_top_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.735" rpy="0 0 0"/>
<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">
</link>
<link name="link3_bottom">
<collision>
<geometry>
<cylinder radius=".05" length=".10"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_3_bottom_coll.stl"/>
</geometry>
</collision>
<visual name="visual">
<visual>
<geometry>
<cylinder radius=".05" length=".10"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_3_bottom_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.86" rpy="0 0 0"/>
<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">
</link>
<link name="link3_top">
<collision>
<geometry>
<cylinder radius=".05" length=".0605"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_3_top_coll.stl"/>
</geometry>
</collision>
<visual name="visual">
<visual>
<geometry>
<cylinder radius=".05" length=".0605"/>
<mesh filename="file://$(find gaz_simulation)/stl/link_3_top_low.stl"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.94025" rpy="0 0 0"/>
<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>
<joint type="fixed" name="world_base_link_fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
<joint type="fixed" name="base_base_link_fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="base"/>
</joint>
<joint type="continuous" name="base_rot">
<origin xyz="0 0 0.1205" rpy="0 0 0"/>
<parent link="base"/>
<child link="base_rot"/>
<axis>
<xyz>0 0 1</xyz>
</axis>
<child link="base_top"/>
<axis xyz="0 0 1"/>
<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 type="revolute" name="base_link1_joint">
<origin xyz="0 -0.080499 0.092997" rpy="0 0 0"/>
<parent link="base_top"/>
<child link="link1_full"/>
<axis xyz="0 1 0"/>
<limit effort="30" velocity="1.7453292519943" lower="-2.2689280275926" upper="2.4434609527921"/>
</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 type="revolute" name="link1_link2_joint">
<origin xyz="0 0 0.3" rpy="0 0 0"/>
<parent link="link1_full"/>
<child link="link2_bottom"/>
<axis xyz="0 1 0"/>
<limit effort="30" velocity="1.7453292519943" lower="-2.6179938779915" upper="2.6179938779915"/>
</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>
<joint type="continuous" name="link2_rot">
<origin xyz="0 0.080501 0.120502" rpy="0 0 0"/>
<parent link="link2_bottom"/>
<child link="link2_top"/>
<axis xyz="0 0 1"/>
<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>
<joint type="continuous" name="link2_link3_joint">
<origin xyz="0 0.0565 0.178003" rpy="0 0 0"/>
<parent link="link2_top"/>
<child link="link3_bottom"/>
<axis xyz="0 1 0"/>
<limit effort="30" velocity="1.7453292519943"/>
</joint>
<joint type="continuous" name="link3_rot">
<origin xyz="0 -0.055001 0.085501" rpy="0 0 0"/>
<parent link="link3_bottom"/>
<child link="link3_top"/>
<axis xyz="0 0 1"/>
<limit effort="30" velocity="1.7453292519943"/>
</joint>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/simple_model</robotNamespace>
</plugin>
</gazebo>
<transmission name="trans_base_rot">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_rot">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="base_rot_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_base_link1_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="base_link1_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="base_link1_joint_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_link1_link2_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link1_link2_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="link1_link2_joint_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_link2_rot">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link2_rot">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="link2_rot_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_link2_link3_joint">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link2_link3_joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="link2_link3_joint_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_link3_rot">
<type>transmission_interface/SimpleTransmission</type>
<joint name="link3_rot">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="link3_rot_motor">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/</robotNamespace>
</plugin>
</gazebo>
</robot>

15
src/gaz_simulation/world/test2.xml
View File

@ -111,7 +111,7 @@
</box>
</geometry>
</visual>-->
<sensor name="laser" type="gpu_ray">
<sensor name="laser_1" type="gpu_ray">
<pose>0.1 0 0 0 0 0</pose>
<ray>
<scan>
@ -132,7 +132,7 @@
<update_rate>10</update_rate>
<visualize>true</visualize>
<plugin name='laser' filename='libgazebo_ros_ray_sensor.so'>
<plugin name='laser_1_plugin' filename='libgazebo_ros_ray_sensor.so'>
<ros>
<namespace>/lidar_1</namespace>
<argument>--ros-args --remap ~/out:=scan</argument>
@ -145,8 +145,8 @@
<model name="lidar_2">
<static>1</static>
<link name="lidar_2_link">
<pose>-2 1 1 0 0 </pose>
<collision name='collision'>
<pose>-1.9 1 1 0 0 </pose>
<!--<collision name='collision'>
<geometry>
<box>
<size>.1 .1 .1</size>
@ -159,9 +159,8 @@
<size>.1 .1 .1</size>
</box>
</geometry>
</visual>
<sensor name="laser" type="gpu_ray">
<pose>0.1 0 0 0 0 0</pose>
</visual>-->
<sensor name="laser_2" type="gpu_ray">
<ray>
<scan>
<horizontal>
@ -181,7 +180,7 @@
<update_rate>10</update_rate>
<visualize>true</visualize>
<plugin name='laser' filename='libgazebo_ros_ray_sensor.so'>
<plugin name='laser_2_plugin' filename='libgazebo_ros_ray_sensor.so'>
<ros>
<namespace>/lidar_2</namespace>
<frameName>/lidar_2_link</frameName>

4
src/gazebo_ros_actor/src/PosePublisher.cc
View File

@ -8,10 +8,10 @@
class PosePublisher : public rclcpp::Node{
public:
PosePublisher(): Node("minimal_publisher") {
PosePublisher(): Node("gazebo_actor_current") {
publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("currentPose",10);
}
void update(ignition::math::Pose3d newPose){
void update(const ignition::math::Pose3d& newPose){
geometry_msgs::msg::Pose pose;
pose.position.x=newPose.X();
pose.position.y=newPose.Y();

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

@ -38,16 +38,16 @@ RosActorPlugin::RosActorPlugin()
class PoseSubscriber : public rclcpp::Node{
public:
explicit PoseSubscriber(RosActorPlugin* plugin): Node("targetPose"){
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>(
"actorTarget", 10, [this](const geometry_msgs::msg::Pose PH1) {
"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,PH1.position.z);
plugin->target.Set(PH1.position.x,PH1.position.y,1.0);
});
};
@ -160,6 +160,9 @@ void RosActorPlugin::HandleObstacles(ignition::math::Vector3d &_pos)
/////////////////////////////////////////////////
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();
@ -169,7 +172,7 @@ void RosActorPlugin::OnUpdate(const common::UpdateInfo &_info)
double distance = pos.Length();
if (distance < 0.3)
if (distance < 0.01)
{
return;
}
@ -178,7 +181,7 @@ void RosActorPlugin::OnUpdate(const common::UpdateInfo &_info)
pos = pos.Normalize() * this->targetWeight;
// Adjust the direction vector by avoiding obstacles
this->HandleObstacles(pos);
// this->HandleObstacles(pos);
// Compute the yaw orientation
ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();

1
src/moveit2_tutorials Submodule

@ -0,0 +1 @@
Subproject commit e02236a8096f540c356504a5ceeb1a08212421e9

1
src/moveit_visual_tools Submodule

@ -0,0 +1 @@
Subproject commit ef16670fab41acb5a9f265ce2de6d7875c6358eb