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

Bugfixes for nodes

Browse Source
This commit is contained in:
Bastian Hofmann 2023-03-31 12:46:35 +00:00
parent 5b54db02a9
commit b558f8d401
3 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/btree_nodes/src/nodes/ActorAnimation.cpp
View File

@ -44,12 +44,16 @@ BT::NodeStatus ActorAnimation::onStart() {
client->async_send_goal(goal,send_goal_options); client->async_send_goal(goal,send_goal_options);
result = BT::NodeStatus::RUNNING;
return BT::NodeStatus::RUNNING; return BT::NodeStatus::RUNNING;
} }
BT::NodeStatus ActorAnimation::onRunning() { BT::NodeStatus ActorAnimation::onRunning() {
mutex.lock(); mutex.lock();
auto status = result; auto status = result;
if(result != BT::NodeStatus::RUNNING){
result = BT::NodeStatus::IDLE;
}
mutex.unlock(); mutex.unlock();
return status; return status;
} }

26
src/btree_nodes/src/nodes/OffsetPose.cpp
View File

@ -3,14 +3,16 @@
// //
#include "OffsetPose.h" #include "OffsetPose.h"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <memory>
BT::PortsList BT::OffsetPose::providedPorts() { BT::PortsList BT::OffsetPose::providedPorts() {
return { return {
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Position2D"), InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("input", "initial position as Pose"),
InputPort<std::shared_ptr<geometry_msgs::msg::Point>>("offset", "offset as a Point"), InputPort<std::shared_ptr<geometry_msgs::msg::Point>>("offset", "offset as a Point"),
InputPort<std::shared_ptr<geometry_msgs::msg::Quaternion>>("orientation", "rotation of resulting pose as Quaternion"), InputPort<std::shared_ptr<geometry_msgs::msg::Quaternion>>("orientation", "rotation of resulting pose as Quaternion"),
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose") OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output", "generated new pose")
}; };
} }
@ -18,28 +20,30 @@ BT::OffsetPose::OffsetPose(const std::string &name,
const BT::NodeConfiguration &config) : SyncActionNode(name, config) {} const BT::NodeConfiguration &config) : SyncActionNode(name, config) {}
BT::NodeStatus BT::OffsetPose::tick() { BT::NodeStatus BT::OffsetPose::tick() {
std::shared_ptr<geometry_msgs::msg::Pose> pose; std::shared_ptr<geometry_msgs::msg::Pose> pose;
if (!getInput("input", pose)) { if (!getInput("input", pose)) {
return NodeStatus::FAILURE; return NodeStatus::FAILURE;
} }
auto resultPose = std::make_shared<geometry_msgs::msg::Pose>(*pose);
std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion; std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion;
if(getInput("orientation", quaternion)) { if(getInput("orientation", quaternion)) {
pose->orientation.w = quaternion->w; resultPose->orientation.w = quaternion->w;
pose->orientation.x = quaternion->x; resultPose->orientation.x = quaternion->x;
pose->orientation.y = quaternion->y; resultPose->orientation.y = quaternion->y;
pose->orientation.z = quaternion->z; resultPose->orientation.z = quaternion->z;
} }
std::shared_ptr<geometry_msgs::msg::Point> offset; std::shared_ptr<geometry_msgs::msg::Point> offset;
if(getInput("offset", offset)) { if(getInput("offset", offset)) {
pose->position.x += offset->x; resultPose->position.x += offset->x;
pose->position.y += offset->y; resultPose->position.y += offset->y;
pose->position.z += offset->z; resultPose->position.z += offset->z;
} }
setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("output", pose); setOutput("output", resultPose);
return BT::NodeStatus::SUCCESS; return BT::NodeStatus::SUCCESS;
} }

4
src/btree_nodes/src/nodes/RobotMove.cpp
View File

@ -13,7 +13,7 @@ BT::RobotMove::RobotMove(const std::string &name, const BT::NodeConfiguration &c
BT::NodeStatus BT::RobotMove::onStart() { BT::NodeStatus BT::RobotMove::onStart() {
std::cout << "Starting RobotMove" << std::endl; std::cout << "Starting RobotMove" << std::endl;
std::shared_ptr<geometry_msgs::msg::Pose> pose; std::shared_ptr<geometry_msgs::msg::Pose> pose;
if(!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target",pose)){ if(!getInput("target",pose)){
std::cout << "no target available." << std::endl; std::cout << "no target available." << std::endl;
return NodeStatus::FAILURE; return NodeStatus::FAILURE;
} }
@ -22,8 +22,10 @@ BT::NodeStatus BT::RobotMove::onStart() {
asyncResult = NodeStatus::RUNNING; asyncResult = NodeStatus::RUNNING;
connection->get()->planAndExecute(pose,[this](bool finished){ connection->get()->planAndExecute(pose,[this](bool finished){
if(finished){ if(finished){
printf("Finished move.");
asyncResult = NodeStatus::SUCCESS; asyncResult = NodeStatus::SUCCESS;
} else { } else {
printf("Failed move.");
asyncResult = NodeStatus::FAILURE; asyncResult = NodeStatus::FAILURE;
} }
}); });