Moveit using BehaviorTree #727
Description
I am trying to experiment behaviour tree with moveit instead of a state machine and right now i do not know why but it is always stuck in the ROS_INFO("hello i am moving"). Could this be a spin issue? or something with behavior tree? I am not sure
using namespace BT;
// Mutex for synchronizing access to the detected_objects vector
std::mutex detected_objects_mutex;
std::vector<geometry_msgs::PoseStamped> detected_objects;
// Callback function for the object detection subscriber
void ObjectDetectionCallback(const aricc_vision_msgs::ObjectArray::ConstPtr& msg)
{
std::lock_guard<std::mutex> lock(detected_objects_mutex);
int cnt = 0;
for (auto& obj : msg->objects)
{
geometry_msgs::TransformStamped transformStamped;
static tf2_ros::TransformBroadcaster tf_broadcaster;
std::cout << "[" << ++cnt << "] Detect object: '" << obj.name << "'." << std::endl;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = obj.name;
pose.pose.position = obj.position;
// Fix: Assign Quaternion components individually
pose.pose.orientation.x = obj.orientation.x;
pose.pose.orientation.y = obj.orientation.y;
pose.pose.orientation.z = obj.orientation.z;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "camera_color_optical_frame"; // Parent frame
transformStamped.child_frame_id = pose.header.frame_id; // Child frame
transformStamped.transform.translation.x = -pose.pose.position.y; // Object position x
transformStamped.transform.translation.y = -pose.pose.position.x; // Object position y
transformStamped.transform.translation.z = pose.pose.position.z;
tf2::Quaternion q;
q.setRPY(1.57, -1.57, -pose.pose.orientation.z);
transformStamped.transform.rotation.x = q.x(); // Rotation
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
detected_objects.push_back(pose);
tf_broadcaster.sendTransform(transformStamped);
}
}
// ObjectDetection class definition
class ObjectDetection : public BT::SyncActionNode
{
public:
ObjectDetection(const std::string& name, const BT::NodeConfiguration& config)
: BT::SyncActionNode(name, config), node_(""), listener_(buffer_)
{
// Subscriber for object detection
sub_ = node_.subscribe("/re_dl_detection/YOLOv8RotateObjectDetection/dl_output", 1000, ObjectDetectionCallback);
}
// Define the input ports for the behavior tree node
static BT::PortsList providedPorts()
{
return {
BT::InputPort<std::string>("object"),
BT::OutputPort<geometry_msgs::Pose>("object_pose")
};
}
// Execute the behavior tree node logic
virtual BT::NodeStatus tick() override
{
std::string object_name;
ros::Duration(1).sleep();
// Check if the required input "object" is missing
if (!getInput<std::string>("object", object_name))
{
throw BT::RuntimeError("missing required input [pose]");
}
{
std::lock_guard<std::mutex> lock(detected_objects_mutex);
detected_objects.clear();
}
ros::spinOnce();
auto predicate = [&object_name](const geometry_msgs::PoseStamped& detected_pose) {
return detected_pose.header.frame_id == object_name;
};
{
std::lock_guard<std::mutex> lock(detected_objects_mutex);
int cnt = std::count_if(detected_objects.begin(), detected_objects.end(), predicate);
if (cnt > 2)
{
try {
// Lookup the transform
buffer_.canTransform("base_link1", detected_objects[0].header.frame_id, ros::Time(0), ros::Duration(1.0));
// Now perform the transform lookup
transformStamped1 = buffer_.lookupTransform("base_link1", detected_objects[0].header.frame_id, ros::Time(0));
} catch (tf2::TransformException &ex) {
ROS_ERROR("Failed to lookup transform: %s", ex.what());
return BT::NodeStatus::FAILURE;
}
// Extract and print the transform information
move_target.position.x = transformStamped1.transform.translation.x;
move_target.position.y = transformStamped1.transform.translation.y;
move_target.position.z = transformStamped1.transform.translation.z;
move_target.orientation.x = transformStamped1.transform.rotation.x;
move_target.orientation.y = transformStamped1.transform.rotation.y;
move_target.orientation.z = transformStamped1.transform.rotation.z;
move_target.orientation.w = transformStamped1.transform.rotation.w;
ROS_INFO_STREAM("Transform from base_link1 to " << detected_objects[0].header.frame_id << " received:");
ROS_INFO_STREAM(move_target);
setOutput<geometry_msgs::Pose>("object_pose" , move_target);
return BT::NodeStatus::SUCCESS;
}
}
return BT::NodeStatus::FAILURE;
}
private:
ros::NodeHandle node_;
ros::Subscriber sub_;
tf2_ros::Buffer buffer_;
tf2_ros::TransformListener listener_;
geometry_msgs::Pose move_target;
geometry_msgs::TransformStamped transformStamped1;
};
class PickObject : public BT::SyncActionNode
{
public:
PickObject(const std::string& name, const BT::NodeConfiguration& config)
: BT::SyncActionNode(name, config) , move_group("gluon_arm")
{}
// Define the input ports for the behavior tree node
static BT::PortsList providedPorts()
{
return {BT::InputPort<geometry_msgs::Pose>("PickObject")};
}
// Execute the behavior tree node logic
virtual BT::NodeStatus tick() override
{
if (!getInput<geometry_msgs::Pose>("PickObject", Pick_Object_))
{
throw BT::RuntimeError("missing required input [pose]");
}
ROS_INFO_STREAM(Pick_Object_);
move_group.setPlanningTime(5.0);
ROS_INFO("hello im moving");
move_group.setPoseReferenceFrame("base_link1");
move_group.setPoseTarget(Pick_Object_);
Pick_Object_.position.z = 0.1;
move_group.move();
ROS_INFO("i have finish movinig");
return BT::NodeStatus::SUCCESS;
}
private:
moveit::planning_interface::MoveGroupInterface move_group;
geometry_msgs::Pose Pick_Object_;
};
// Main function
int main(int argc, char** argv)
{
ros::init(argc, argv, "test_bt");
ros::NodeHandle nh;
// Behavior Tree Factory
BehaviorTreeFactory factory;
factory.registerNodeType<ObjectDetection>("ObjectDetection");
factory.registerNodeType<PickObject>("PickObject");
// Create Behavior Tree from XML file
auto tree = factory.createTreeFromFile("/home/ubuntu/catkin_ws_qt/src/test_node/cfg/bt_tree.xml");
// Logger for console output
StdCoutLogger logger_cout(tree);
NodeStatus status = NodeStatus::RUNNING;
// Run the Behavior Tree
while (ros::ok() && status == NodeStatus::RUNNING)
{
status = tree.rootNode()->executeTick();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
return 0;
}