Skip to content

Behavior Plugin#

Behavior plugin enables support multiple type of NPC behaviors. Default behavior plugin implementation is here.

Behavior plugin use pluginlib and plugin class should be inherited this base class.

Example of the behavior plugin class is below.

// Copyright 2015 TIER IV, Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef BEHAVIOR_TREE_PLUGIN__PEDESTRIAN__BEHAVIOR_TREE_HPP_
#define BEHAVIOR_TREE_PLUGIN__PEDESTRIAN__BEHAVIOR_TREE_HPP_

#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/loggers/bt_cout_logger.h>

#include <behavior_tree_plugin/pedestrian/follow_lane_action.hpp>
#include <behavior_tree_plugin/pedestrian/walk_straight_action.hpp>
#include <behavior_tree_plugin/transition_events/transition_events.hpp>
#include <functional>
#include <geometry_msgs/msg/point.hpp>
#include <map>
#include <memory>
#include <optional>
#include <string>
#include <traffic_simulator/behavior/behavior_plugin_base.hpp>
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
#include <traffic_simulator_msgs/msg/entity_status.hpp>
#include <vector>
#include <visualization_msgs/msg/marker_array.hpp>

namespace entity_behavior
{
class PedestrianBehaviorTree : public BehaviorPluginBase
{
public:
  void configure(const rclcpp::Logger & logger) override;
  void update(double current_time, double step_time) override;
  const std::string & getCurrentAction() const override;
#define DEFINE_GETTER_SETTER(NAME, TYPE)                                                    \
  TYPE get##NAME() override { return tree_.rootBlackboard()->get<TYPE>(get##NAME##Key()); } \
  void set##NAME(const TYPE & value) override                                               \
  {                                                                                         \
    tree_.rootBlackboard()->set<TYPE>(get##NAME##Key(), value);                             \
  }

  // clang-format off
  DEFINE_GETTER_SETTER(CurrentTime, double)
  DEFINE_GETTER_SETTER(DebugMarker, std::vector<visualization_msgs::msg::Marker>)
  DEFINE_GETTER_SETTER(DriverModel, traffic_simulator_msgs::msg::DriverModel)
  DEFINE_GETTER_SETTER(EntityStatus, traffic_simulator_msgs::msg::EntityStatus)
  DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr<hdmap_utils::HdMapUtils>)
  DEFINE_GETTER_SETTER(Obstacle, std::optional<traffic_simulator_msgs::msg::Obstacle>)
  DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict)
  DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters)
  DEFINE_GETTER_SETTER(Request, std::string)
  DEFINE_GETTER_SETTER(RouteLanelets, std::vector<std::int64_t>)
  DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr<math::geometry::CatmullRomSpline>)
  DEFINE_GETTER_SETTER(StepTime, double)
  DEFINE_GETTER_SETTER(TargetSpeed, std::optional<double>)
  DEFINE_GETTER_SETTER(ToLaneletId, std::int64_t)
  DEFINE_GETTER_SETTER(TrafficLightManager,std::shared_ptr<traffic_simulator::TrafficLightManager>)
  DEFINE_GETTER_SETTER(UpdatedStatus, traffic_simulator_msgs::msg::EntityStatus)
  DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters)
  DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray)
  // clang-format on

#undef DEFINE_GETTER_SETTER

private:
  BT::NodeStatus tickOnce(double current_time, double step_time);
  BT::BehaviorTreeFactory factory_;
  BT::Tree tree_;
  std::shared_ptr<behavior_tree_plugin::LoggingEvent> logging_event_ptr_;
  std::shared_ptr<behavior_tree_plugin::ResetRequestEvent> reset_request_event_ptr_;
};
}  // namespace entity_behavior

#endif  // BEHAVIOR_TREE_PLUGIN__PEDESTRIAN__BEHAVIOR_TREE_HPP_

If you want to develop behavior plugin, first, you have to add three functions below.

void configure(const rclcpp::Logger & logger) override;
configure plugin class, this function only calls once at the start of simulation.
void update(double current_time, double step_time) override;
update plugin class, this function calls once in every frame in simulation.
const std::string & getCurrentAction() const override;
return current actions of NPC, this result only use for visualization.

After that, you have to define getter and setters for each data.
You can get unique key for each data by calling BehaviorPluginBase::get(FOO)Key function, such as BehaviorPluginBase::getCurrentTimeKey().
Getter are named as get(FOO), such as BehaviorPluginClass::getDriverModel();.
Setters are named as set(Foo), such as BehaviorPluginClass::setDriverModel(traffic_simulator_msgs::msg::DriverModel);.

Name Description Type
DebugMarker Rviz marker for debugging NPC behavior std::vector<visualization_msgs::msg::Marker>
DriverModel Driver behavior parameters traffic_simulator_msgs::msg::DriverModel
EntityStatus Entity status of the NPC you want to control std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>
GoalPoses Goal poses of entity. std::vector<geometry_msgs::msg::Pose>
HdMapUtils Shared pointer of HdMapUtils class. std::shared_ptr<hdmap_utils::HdMapUtils>
Obstacle Target obstacle of your NPC. std::optional<traffic_simulator_msgs::msg::Obstacle>
OtherEntityStatus Dictionary of other entity status. EntityStatusDict
PedestrianParameters Entity parameters for pedestrian. traffic_simulator_msgs::msg::PedestrianParameters
Request Request to the NPC you want to control std::string
RouteLanelets Lanelet ids on entity route std::vector<std::int64_t>
ReferenceTrajectory Trajectory precalculated for RouteLanelets. std::shared_ptr<math::geometry::CatmullRomSpline>
StepTime Step time of the simulation. double
TargetSpeed Target speed of the NPC you want to control std::optional<double>
ToLaneletId Goal lanelet ID std::int64_t
TrafficLightManager Shared pointer to the traffic light manager std::shared_ptr<traffic_simulator::TrafficLightManager>
UpdatedStatus Updated entity status of the NPC you want to control. std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>
VehicleParameters Parameters of vehicle entity. traffic_simulator_msgs::msg::VehicleParameters
Waypoints Waypoints of the NPC you want to control. traffic_simulator_msgs::msg::WaypointsArray