Skip to content

Commit

Permalink
fix compile erro in spawn function
Browse files Browse the repository at this point in the history
Signed-off-by: Masaya Kataoka <[email protected]>
  • Loading branch information
hakuturu583 committed Feb 10, 2025
1 parent 1f8227d commit 8a76a41
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 37 deletions.
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
# ament_auto_add_executable(spawn
# spawn.cpp
# )
# target_link_libraries(spawn
# ${PROTOBUF_LIBRARY}
# pthread
# sodium
# zmq
# )
ament_auto_add_executable(spawn
spawn.cpp
)
target_link_libraries(spawn
${PROTOBUF_LIBRARY}
pthread
sodium
zmq
)

ament_auto_add_executable(stop_at_crosswalk
stop_at_crosswalk.cpp
Expand Down
48 changes: 20 additions & 28 deletions mock/context_gamma_scenarios/src/context_gamma_planner/spawn.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,8 @@
#include <quaternion_operation/quaternion_operation.h>

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <context_gamma_scenarios/catalogs.hpp>
#include <context_gamma_scenarios/context_gamma_scenario_node.hpp>
#include <cpp_mock_scenarios/catalogs.hpp>
#include <cpp_mock_scenarios/cpp_scenario_node.hpp>
#include <rclcpp/rclcpp.hpp>
#include <traffic_simulator/api/api.hpp>

Expand All @@ -25,11 +25,11 @@
#include <string>
#include <vector>

class SpawnScenario : public context_gamma_scenarios::ContextGammaScenarioNode
class SpawnScenario : public cpp_mock_scenarios::CppScenarioNode
{
public:
explicit SpawnScenario(const rclcpp::NodeOptions & option)
: context_gamma_scenarios::ContextGammaScenarioNode(
: cpp_mock_scenarios::CppScenarioNode(
"spawn", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map",
"lanelet2_map.osm", __FILE__, false, option)
{
Expand All @@ -43,47 +43,39 @@ class SpawnScenario : public context_gamma_scenarios::ContextGammaScenarioNode

// LCOV_EXCL_STOP
if (t >= 5) {
stop(context_gamma_scenarios::Result::SUCCESS);
stop(cpp_mock_scenarios::Result::SUCCESS);
}
}

void onInitialize() override
{
//Vehicle setting
api_.spawn(
"ego",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
120545, 0, 0, api_.getHdmapUtils()),
"ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0, 0),
getVehicleParameters(), traffic_simulator::VehicleBehavior::contextGamma());
api_.setEntityStatus(
"ego",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
120545, 0, 0, api_.getHdmapUtils()),
auto & ego = api_.getEntity("ego");
ego.setStatus(
traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0, 0),
traffic_simulator::helper::constructActionStatus(10));
api_.requestSpeedChange("ego", 0, true);
api_.requestAssignRoute(
"ego", std::vector<traffic_simulator::CanonicalizedLaneletPose>{
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34675, 0.0, 0, api_.getHdmapUtils()),
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34690, 0.0, 0, api_.getHdmapUtils()),
});
ego.requestSpeedChange(0, true);
ego.requestAssignRoute(std::vector<traffic_simulator::CanonicalizedLaneletPose>{
traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0),
traffic_simulator::helper::constructCanonicalizedLaneletPose(34690, 0.0, 0),
});

//Pedestrian setting
api_.spawn(
"bob",
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34378, 0.0, 0, api_.getHdmapUtils()),
"bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 0.0, 0),
getPedestrianParameters(), traffic_simulator::PedestrianBehavior::contextGamma());
api_.requestSpeedChange(
"bob", 0.0, traffic_simulator::speed_change::Transition::LINEAR,
auto & bob = api_.getEntity("bob");
bob.requestSpeedChange(
0.0, traffic_simulator::speed_change::Transition::LINEAR,
traffic_simulator::speed_change::Constraint(
traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 1.0),
true);
const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose(
traffic_simulator::helper::constructCanonicalizedLaneletPose(
34378, 7.5, 0, 0, 0, 0, api_.getHdmapUtils()));
api_.requestAcquirePosition("bob", goal_pose);
traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 7.5, 0, 0, 0, 0));
bob.requestAcquirePosition(goal_pose);
}
};

Expand Down

0 comments on commit 8a76a41

Please sign in to comment.