-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
progress nav API and MVSIM interface
- Loading branch information
Showing
5 changed files
with
271 additions
and
53 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
80 changes: 80 additions & 0 deletions
80
libselfdriving/include/selfdriving/interfaces/MVSIM_VehicleInterface.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,80 @@ | ||
| /* ------------------------------------------------------------------------- | ||
| * SelfDriving C++ library based on PTGs and mrpt-nav | ||
| * Copyright (C) 2019-2021 Jose Luis Blanco, University of Almeria | ||
| * See LICENSE for license information. | ||
| * ------------------------------------------------------------------------- */ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <mvsim/Comms/Client.h> | ||
| #include <mvsim/mvsim-msgs/SrvGetPose.pb.h> | ||
| #include <mvsim/mvsim-msgs/SrvGetPoseAnswer.pb.h> | ||
| #include <selfdriving/interfaces/VehicleMotionInterface.h> | ||
|
|
||
| namespace selfdriving | ||
| { | ||
| /** Vehicle adaptor class for the MVSIM simulator. | ||
| * | ||
| */ | ||
| class MVSIM_VehicleInterface : public VehicleMotionInterface | ||
| { | ||
| public: | ||
| MVSIM_VehicleInterface() {} | ||
|
|
||
| /** Connect to the MVSIM server. | ||
| */ | ||
| void connect() | ||
| { | ||
| MRPT_LOG_INFO("Connecting to mvsim server..."); | ||
| connection_.connect(); | ||
| MRPT_LOG_INFO("Connected OK."); | ||
| } | ||
|
|
||
| // See base class docs | ||
| VehicleLocalizationState get_localization() override | ||
| { | ||
| mvsim_msgs::SrvGetPose req; | ||
| req.set_objectid(robotName_); | ||
|
|
||
| mvsim_msgs::SrvGetPoseAnswer ans; | ||
| connection_.callService("get_pose", req, ans); | ||
|
|
||
| VehicleLocalizationState vls; | ||
| vls.frame_id = "map"; | ||
| vls.timestamp = mrpt::Clock::now(); | ||
| vls.valid = true; | ||
|
|
||
| vls.pose.x = ans.pose().x(); | ||
| vls.pose.y = ans.pose().y(); | ||
| vls.pose.phi = ans.pose().yaw(); | ||
|
|
||
| return vls; | ||
| } | ||
|
|
||
| // See base class docs | ||
| VehicleOdometryState get_odometry() override | ||
| { | ||
| // | ||
| return {}; | ||
| } | ||
|
|
||
| // See base class docs | ||
| bool motion_execute( | ||
| const std::optional<CVehicleVelCmd::Ptr>& immediate, | ||
| const std::optional<EnqueuedMotionCmd>& next) override | ||
| { | ||
| return true; | ||
| } | ||
|
|
||
| // See base class docs | ||
| void stop(const STOP_TYPE stopType) override | ||
| { | ||
| // | ||
| } | ||
|
|
||
| private: | ||
| mvsim::Client connection_{"MVSIM_VehicleInterface"}; | ||
| std::string robotName_ = "r1"; | ||
| }; | ||
|
|
||
| } // namespace selfdriving |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.