Never changed ActorPlugin to Action, instead minor renaming and tweaks.
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
parent
3f9b39f9d6
commit
80da2d4153
@ -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)
|
||||
|
||||
@ -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,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();
|
||||
});
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
typedef std::chrono::milliseconds Milliseconds;
|
||||
|
||||
TimePoint Now() {
|
||||
return std::chrono::high_resolution_clock::now();
|
||||
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
70
src/btree_nodes/src/WeightedRandomNode.cpp
Normal 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
35
src/btree_nodes/src/WeightedRandomNode.h
Normal 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
42
src/btree_nodes/tree.xml
Normal 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>
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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,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
BIN
src/gaz_simulation/stl/base_bottom_coll.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/base_bottom_low.stl
Executable file
BIN
src/gaz_simulation/stl/base_bottom_low.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/base_top_low.stl
Executable file
BIN
src/gaz_simulation/stl/base_top_low.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_1_full_coll.stl
Executable file
BIN
src/gaz_simulation/stl/link_1_full_coll.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_1_full_low.stl
Executable file
BIN
src/gaz_simulation/stl/link_1_full_low.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_2_bottom_low.stl
Executable file
BIN
src/gaz_simulation/stl/link_2_bottom_low.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_2_top_coll.stl
Executable file
BIN
src/gaz_simulation/stl/link_2_top_coll.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_2_top_low.stl
Executable file
BIN
src/gaz_simulation/stl/link_2_top_low.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_3_bottom_coll.stl
Executable file
BIN
src/gaz_simulation/stl/link_3_bottom_coll.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_3_bottom_low.stl
Executable file
BIN
src/gaz_simulation/stl/link_3_bottom_low.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_3_top_coll.stl
Executable file
BIN
src/gaz_simulation/stl/link_3_top_coll.stl
Executable file
Binary file not shown.
BIN
src/gaz_simulation/stl/link_3_top_low.stl
Executable file
BIN
src/gaz_simulation/stl/link_3_top_low.stl
Executable file
Binary file not shown.
@ -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'>
|
||||
<?xml version="1.0" ?>
|
||||
<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>
|
||||
@ -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>
|
||||
|
||||
@ -8,10 +8,10 @@
|
||||
|
||||
class PosePublisher : public rclcpp::Node{
|
||||
public:
|
||||
PosePublisher(): Node("minimal_publisher") {
|
||||
publisher_ = this->create_publisher<geometry_msgs::msg::Pose>("currentPose", 10);
|
||||
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();
|
||||
|
||||
@ -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
1
src/moveit2_tutorials
Submodule
@ -0,0 +1 @@
|
||||
Subproject commit e02236a8096f540c356504a5ceeb1a08212421e9
|
||||
1
src/moveit_visual_tools
Submodule
1
src/moveit_visual_tools
Submodule
@ -0,0 +1 @@
|
||||
Subproject commit ef16670fab41acb5a9f265ce2de6d7875c6358eb
|
||||
Loading…
x
Reference in New Issue
Block a user