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);
result = BT::NodeStatus::RUNNING;
return BT::NodeStatus::RUNNING;
}
BT::NodeStatus ActorAnimation::onRunning() {
mutex.lock();
auto status = result;
if(result != BT::NodeStatus::RUNNING){
result = BT::NodeStatus::IDLE;
}
mutex.unlock();
return status;
}

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

@ -3,14 +3,16 @@
//
#include "OffsetPose.h"
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <memory>
BT::PortsList BT::OffsetPose::providedPorts() {
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::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) {}
BT::NodeStatus BT::OffsetPose::tick() {
std::shared_ptr<geometry_msgs::msg::Pose> pose;
if (!getInput("input", pose)) {
return NodeStatus::FAILURE;
}
auto resultPose = std::make_shared<geometry_msgs::msg::Pose>(*pose);
std::shared_ptr<geometry_msgs::msg::Quaternion> quaternion;
if(getInput("orientation", quaternion)) {
pose->orientation.w = quaternion->w;
pose->orientation.x = quaternion->x;
pose->orientation.y = quaternion->y;
pose->orientation.z = quaternion->z;
resultPose->orientation.w = quaternion->w;
resultPose->orientation.x = quaternion->x;
resultPose->orientation.y = quaternion->y;
resultPose->orientation.z = quaternion->z;
}
std::shared_ptr<geometry_msgs::msg::Point> offset;
if(getInput("offset", offset)) {
pose->position.x += offset->x;
pose->position.y += offset->y;
pose->position.z += offset->z;
resultPose->position.x += offset->x;
resultPose->position.y += offset->y;
resultPose->position.z += offset->z;
}
setOutput<std::shared_ptr<geometry_msgs::msg::Pose>>("output", pose);
setOutput("output", resultPose);
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() {
std::cout << "Starting RobotMove" << std::endl;
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;
return NodeStatus::FAILURE;
}
@ -22,8 +22,10 @@ BT::NodeStatus BT::RobotMove::onStart() {
asyncResult = NodeStatus::RUNNING;
connection->get()->planAndExecute(pose,[this](bool finished){
if(finished){
printf("Finished move.");
asyncResult = NodeStatus::SUCCESS;
} else {
printf("Failed move.");
asyncResult = NodeStatus::FAILURE;
}
});