liuhaoyan
/
webots_simulation
1
0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

merge into: liuhaoyan:master
Branches Tags
liuhaoyan:master
liuhaoyan:main
...
pull from: liuhaoyan:main
Branches Tags
liuhaoyan:master
liuhaoyan:main

No commits in common. "master" and "main" have entirely different histories.
master ... main

24 changed files with 67 additions and 532723 deletions
Show Stats Expand all files Collapse all files

65
.gitignore vendored Normal file
View File

@ -0,0 +1,65 @@
# ---> C++
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
# ---> ROS2
install/
log/
build/
# Ignore generated docs
*.dox
*.wikidoc
# eclipse stuff
.project
.cproject
# qcreator stuff
CMakeLists.txt.user
srv/_*.py
*.pcd
*.pyc
qtcreator-*
*.user
*~
# Emacs
.#*
# Colcon custom files
COLCON_IGNORE
AMENT_IGNORE

122
CMakeLists.txt
View File

@ -1,122 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(sweet_webots_robot)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(pluginlib REQUIRED)
find_package(webots_ros2_driver REQUIRED)
find_package(sweet_msg REQUIRED)
find_package(visualization_msgs REQUIRED)
find_package(CURL REQUIRED)
pluginlib_export_plugin_description_file(
webots_ros2_driver
webots_robot_driver.xml
)
add_library(
${PROJECT_NAME}
SHARED
src/WebotsRobotDriver.cpp
)
target_include_directories(
${PROJECT_NAME}
PRIVATE
include
)
add_executable(
path_visualizer
src/PathVisualizer.cpp
)
ament_target_dependencies(
${PROJECT_NAME}
pluginlib
rclcpp
webots_ros2_driver
geometry_msgs
nav_msgs
sweet_msg
visualization_msgs
)
ament_target_dependencies(
path_visualizer
pluginlib
rclcpp
webots_ros2_driver
geometry_msgs
nav_msgs
sweet_msg
visualization_msgs
)
target_include_directories(
path_visualizer
PRIVATE
include
)
target_link_libraries(
path_visualizer
CURL::libcurl
)
ament_target_dependencies(
path_visualizer
rclcpp
geometry_msgs
nav_msgs
)
install(
TARGETS
path_visualizer
DESTINATION lib/${PROJECT_NAME}
)
install(
TARGETS
${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(
DIRECTORY
launch
resource
worlds
DESTINATION share/${PROJECT_NAME}/
)
install(
DIRECTORY
urdf
meshes
launch
DESTINATION share/${PROJECT_NAME}
)
ament_export_include_directories(
include
)
ament_export_libraries(
${PROJECT_NAME}
)
ament_package()

202
LICENSE
View File

@ -1,202 +0,0 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
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.

2
README.md Normal file
View File

@ -0,0 +1,2 @@
# webots_simulation

BIN
communication diagram.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 104 KiB

104
include/sweet_webots_robot/PathVisualizer.hpp
View File

@ -1,104 +0,0 @@
#ifndef PATH_VISUALIZER_HPP_
#define PATH_VISUALIZER_HPP_
#include <memory>
#include <curl/curl.h>
#include <nlohmann/json.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <algorithm>
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include <nav_msgs/msg/path.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
class PathVisualizer : public rclcpp::Node
{
public:
PathVisualizer(const std::string& file_name);
private:
void localozation_callback(const geometry_msgs::msg::Pose::SharedPtr msg);
void control();
double quaternion_to_yaw(double x,
double y,
double z,
double w);
double normalize_angle(double angle);
bool downloadPathJson(const std::string &url,std::string &json_data);
bool loadPathFromJson(const std::string &json_data);
double gps_to_x(double lat,double lon);
double gps_to_y(double lat,double lon);
int search_nearest_point();
int findLookAheadPoint();
int findClosestPoint(int forward_index_limit);
// pub rviz info
void publishPathRviz();
void publishVehiclePoseRviz();
void publishVehicleTFRviz();
void publishHistoryPathRviz();
void publishVehicleInfoRviz();
void transformPathToVehicleStart();
void densifyPath(double interval);
void publishLocalPose();
void publishOdomPath();
private:
// topic pub
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr local_pose_pub_;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr odom_path_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr path_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr history_pose_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_info_pub_;
// topic sub
rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr odom_sub_;
rclcpp::TimerBase::SharedPtr timer_;
// 坐标转换broadcaster
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; // odom -> baselink
private:
// odom path info
std::vector<double> path_x_;
std::vector<double> path_y_;
std::vector<double> path_yaw_;
// odom history pose
std::vector<double> history_x_;
std::vector<double> history_y_;
std::vector<double> history_yaw_;
int target_index_;
// localization info
double x_;
double y_;
double yaw_;
bool has_pose_;
// contorl paramss
double max_speed_ = 0.8;
double max_yaw_rate_ = 2.0;
double lookahead_distance_ =0.6;
double origin_lat_;
double origin_lon_;
double origin_yaw_;
int forward_index_limit_=100;
bool origin_set_= false;
bool path_transed = false;
std::string file_name_;
std::string path_url_ ;
std::string json_data_;
};
#endif

66
include/sweet_webots_robot/WebotsRobotDriver.hpp
View File

@ -1,66 +0,0 @@
#ifndef WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP
#define WEBOTS_ROS2_PLUGIN_EXAMPLE_HPP
#include "rclcpp/macros.hpp"
#include "webots_ros2_driver/PluginInterface.hpp"
#include "webots_ros2_driver/WebotsNode.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "/home/sweetai/webots_simulation/install/sweet_msg/include/sweet_msg/msg/movebase.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include <webots/Speaker.hpp>
#include <nav_msgs/msg/path.hpp>
#include <webots/Node.hpp>
#include <webots/Field.hpp>
#include <array>
#include <vector>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include "sweet_utils/Utils/common_utils.hpp"
namespace webots_robot_driver {
class WebotsRobotDriver : public webots_ros2_driver::PluginInterface {
public:
void step() override;
void init(webots_ros2_driver::WebotsNode *node,
std::unordered_map<std::string, std::string> &parameters) override;
private:
void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
void twist_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg);
void move_base_callback(const sweet_msg::msg::Movebase::SharedPtr move_base_msg);
void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
void getRobotCenterPose(double &x, double &y, const float yaw);
void pub_current_pose(const double lat, const double lon, const float roll, const float pitch, const float yaw);
float get_bearing_in_degrees();
rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr rtk_pose_publisher;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_cmd_vel_sub;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
cmd_vel_subscription_;
rclcpp::Subscription<sweet_msg::msg::Movebase>::SharedPtr move_base_sub;
std::vector<std::array<double, 3>> path_points_;
geometry_msgs::msg::Twist cmd_vel_msg;
WbDeviceTag right_motor;
WbDeviceTag left_motor;
WbDeviceTag speaker;
WbDeviceTag gp, imu, compass;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
} // namespace webots_robot_driver
#endif

BIN
launch/__pycache__/robot_launch.cpython-310.pyc
View File

Binary file not shown.

BIN
launch/__pycache__/robot_launch.cpython-38.pyc
View File

Binary file not shown.

78
launch/robot_launch.py
View File

@ -1,78 +0,0 @@
import os
import pathlib
import launch
from launch_ros.actions import Node
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from webots_ros2_driver.webots_launcher import WebotsLauncher
from webots_ros2_driver.utils import controller_url_prefix
def generate_launch_description():
package_dir = get_package_share_directory('sweet_webots_robot')
controllers_dir = os.path.join( package_dir,'controllers')
robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'sweet_robot.urdf')).read_text()
webots = WebotsLauncher(
world=os.path.join(package_dir, 'worlds', 'sweet_robot.wbt')
#world=os.path.join(package_dir, 'worlds', 'my_world.wbt')
)
my_robot_driver = Node(
package='webots_ros2_driver',
executable='driver',
output='screen',
additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'tracked_robot'},
#additional_env={'WEBOTS_CONTROLLER_URL': controller_url_prefix() + 'my_robot'},
parameters=[
{'robot_description': robot_description},
]
)
pkg_path = get_package_share_directory(
'sweet_webots_robot'
)
urdf_file = os.path.join(
pkg_path,
'urdf',
'rviz_robot.urdf'
)
with open(urdf_file, 'r') as f:
robot_description = f.read()
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
parameters=[
{
'robot_description': robot_description
}
],
output='screen'
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen'
)
path_visualizer = Node(
package='sweet_webots_robot',
executable='path_visualizer',
name='path_visualizer',
output='screen'
)
return LaunchDescription([
webots,
my_robot_driver,
robot_state_publisher,
rviz_node,
#path_visualizer,
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=webots,
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
)
)
])

36
meshes/tracked_robot.mtl
View File

@ -1,36 +0,0 @@
# Material file generated from Webots sweet_robot.wbt
newmtl mat_01
Kd 0.333333 0.341176 0.325490
Ka 0.333333 0.341176 0.325490
Ks 0.0 0.0 0.0
Ns 1.0
d 1.0
newmtl mat_02
Kd 0.979995 0.446784 0.140459
Ka 0.979995 0.446784 0.140459
Ks 0.0 0.0 0.0
Ns 1.0
d 1.0
newmtl mat_03
Kd 0.952941 0.952941 0.952941
Ka 0.952941 0.952941 0.952941
Ks 0.0 0.0 0.0
Ns 1.0
d 1.0
newmtl mat_04
Kd 0.913329 0.368383 0.368383
Ka 0.913329 0.368383 0.368383
Ks 0.0 0.0 0.0
Ns 1.0
d 1.0
newmtl mat_05
Kd 0.700000 0.700000 0.700000
Ka 0.700000 0.700000 0.700000
Ks 0.0 0.0 0.0
Ns 1.0
d 1.0

529718
meshes/tracked_robot.obj
View File

File diff suppressed because it is too large Load Diff

28
package.xml
View File

@ -1,28 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>sweet_webots_robot</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="405649786@qq.com">ahui</maintainer>
<license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>webots_ros2_driver</depend>
<depend>pluginlib</depend>
<depend>sweet_msg</depend>
<depend>sweet_utils</depend>
<depend>visualization_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

23
readme.md
View File

@ -1,23 +0,0 @@
# Sweet_webots_robot
This repository contains the simulation code based on Webots.
And it is communcatied by ros2, visualized by rviz2.
Package name: sweet_webots_robot
# Version
Ros2: foxy
Webots: R2023a
Rviz2
# Communication Schematic
Communication Schematic diagram：
![通信示意图](./communication%20diagram.png)
# Library installation
ros2 related only
# Run
1. rm -rf build install log
2. colcon build
3. source install/setup.bash
4. ros2 launch sweet_webots_robot sweet_webots_robot.launch.py
5. ros2 run sweet_webots_robot path_visualizer

6
resource/sweet_robot.urdf
View File

@ -1,6 +0,0 @@
<?xml version="1.0" ?>
<robot name="My robot">
<webots>
<plugin type="webots_robot_driver::WebotsRobotDriver" />
</webots>
</robot>

818
src/PathVisualizer.cpp
View File

@ -1,818 +0,0 @@
#include "sweet_webots_robot/PathVisualizer.hpp"
PathVisualizer::PathVisualizer(const std::string& file_name):
Node("path_visualizer"),
target_index_(0),
has_pose_(false)
{
file_name_ = file_name;
path_url_ = "https://test.sweetai.cn:4433/load/locationCmdPath/" + file_name_;
RCLCPP_INFO(this->get_logger(),"PathVisualizer started");
// pub
cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/twist_cmd_vel",10);
local_pose_pub_ = this->create_publisher<geometry_msgs::msg::Pose>("/local_pose",10);
odom_path_pub_ = this->create_publisher<nav_msgs::msg::Path>("/odom_path",10);
path_pub_ =this->create_publisher<visualization_msgs::msg::Marker>("/rviz_planning_path",10);
vehicle_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/rviz_vehicle_pose",10);
history_pose_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/rviz_history_traj",10);
vehicle_info_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/rviz_text_info",10);
// sub
RCLCPP_INFO(this->get_logger(),"Start receving rtk pose data");
odom_sub_ = this->create_subscription<geometry_msgs::msg::Pose>("/rtk_pose",10,std::bind(
&PathVisualizer::localozation_callback,this,std::placeholders::_1));
// timer
timer_ =this->create_wall_timer(std::chrono::milliseconds(50),std::bind(
&PathVisualizer::control,this));
// urdf pose
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
// path get
downloadPathJson(path_url_,json_data_);
loadPathFromJson(json_data_);
}
using json = nlohmann::json;
static size_t curlWriteCallback(
void *contents,
size_t size,
size_t nmemb,
void *userp)
{
const size_t total_size = size * nmemb;
std::string *buffer =
static_cast<std::string *>(userp);
buffer->append(
static_cast<char *>(contents),
total_size);
return total_size;
}
void PathVisualizer::transformPathToVehicleStart()
{
if (path_x_.empty())
{
RCLCPP_WARN(this->get_logger(),"Path is empty, cannot transform");
return;
}
if (!origin_set_)
{
RCLCPP_WARN(this->get_logger(),"Vehicle initial pose has not been set");
return;
}
const double tx = gps_to_x(origin_lat_,origin_lon_);
const double ty = gps_to_y(origin_lat_,origin_lon_);
const double theta =origin_yaw_;
const double path_origin_x = path_x_[0];
const double path_origin_y = path_y_[0];
const double yaw_offset = theta - path_yaw_[0];
double normalized_yaw_offset = yaw_offset;
const double cos_theta =std::cos(yaw_offset);
const double sin_theta =std::sin(yaw_offset);
while (normalized_yaw_offset > M_PI)
{
normalized_yaw_offset -= 2.0 * M_PI;
}
while (normalized_yaw_offset < -M_PI)
{
normalized_yaw_offset += 2.0 * M_PI;
}
for (size_t i = 0;i < path_x_.size();++i)
{
path_x_[i] -= path_origin_x;
path_y_[i] -= path_origin_y;
const double x = path_x_[i];
const double y = path_y_[i];
// 旋转 + 平移
path_x_[i] = cos_theta * x - sin_theta * y + tx;
path_y_[i] = sin_theta * x + cos_theta * y + ty;
path_yaw_[i] += normalized_yaw_offset;
// 归一化到 [-pi, pi]
while (path_yaw_[i] > M_PI)
{
path_yaw_[i] -= 2.0 * M_PI;
}
while (path_yaw_[i] < -M_PI)
{
path_yaw_[i] += 2.0 * M_PI;
}
}
RCLCPP_INFO(
this->get_logger(),
"Path transformed to vehicle initial pose");
RCLCPP_INFO(
this->get_logger(),
"Vehicle initial: x=%.3f y=%.3f yaw=%.3f",
tx,
ty,
theta);
RCLCPP_INFO(
this->get_logger(),
"New Path[0]: x=%.3f y=%.3f yaw=%.3f",
path_x_[0],
path_y_[0],
path_yaw_[0]);
}
bool PathVisualizer::downloadPathJson(const std::string &url,
std::string &json_data)
{
CURL *curl = curl_easy_init();
if (!curl)
{
RCLCPP_ERROR(this->get_logger(),"curl_easy_init() failed");
return false;
}
json_data.clear();
curl_easy_setopt(curl,CURLOPT_URL,url.c_str());
curl_easy_setopt(curl,CURLOPT_WRITEFUNCTION,curlWriteCallback);
curl_easy_setopt(curl,CURLOPT_WRITEDATA,&json_data);
// HTTPS
curl_easy_setopt(curl,CURLOPT_SSL_VERIFYPEER, 0L);
curl_easy_setopt(curl,CURLOPT_SSL_VERIFYHOST, 0L);
// 超时
curl_easy_setopt( curl,CURLOPT_CONNECTTIMEOUT,10L);
curl_easy_setopt(curl,CURLOPT_TIMEOUT,30L);
CURLcode result = curl_easy_perform(curl);
if (result != CURLE_OK)
{
RCLCPP_ERROR(this->get_logger(),"Download path failed: %s",curl_easy_strerror(result));
curl_easy_cleanup(curl);
return false;
}
long response_code = 0;
curl_easy_getinfo(curl,CURLINFO_RESPONSE_CODE,&response_code);
curl_easy_cleanup(curl);
if (response_code != 200)
{
RCLCPP_ERROR(this->get_logger(),"HTTP response code = %ld",response_code);
return false;
}
RCLCPP_INFO(this->get_logger(),"Path JSON downloaded successfully, size = %zu", json_data.size());
return true;
}
bool PathVisualizer::loadPathFromJson(
const std::string &json_data)
{
try
{
// 解析 JSON
json data =json::parse(json_data);
// 检查 features
if (!data.contains("features") ||
!data["features"].is_array() ||
data["features"].empty())
{
RCLCPP_ERROR(this->get_logger(),"JSON does not contain valid features");
return false;
}
// 获取 coordinates
const auto &coordinates =
data["features"][0]
["geometry"]
["coordinates"];
if (!coordinates.is_array() ||
coordinates.empty())
{
RCLCPP_ERROR(this->get_logger(),"Path coordinates are empty");
return false;
}
// 清空旧路径
path_x_.clear();
path_y_.clear();
path_yaw_.clear();
// 经纬度 → 局部 XY
for (const auto &point : coordinates)
{
if (!point.is_array() ||point.size() < 3)
{
RCLCPP_WARN(this->get_logger(),"Invalid path point, skip");
continue;
}
// 原始数据
const double lat = point[0].get<double>();
const double lon = point[1].get<double>();
const double yaw =point[2].get<double>();
// 局部 坐标转换
const double x = gps_to_x(lat,lon);
const double y = gps_to_y(lat,lon);
// 保存
path_x_.push_back(x);
path_y_.push_back(y);
path_yaw_.push_back(yaw);
}
// 检查
if (path_x_.empty())
{
RCLCPP_ERROR(this->get_logger(),"No valid path points");
return false;
}
RCLCPP_INFO( this->get_logger(),"Loaded path: %zu points",path_x_.size());
// 输出第一个点
RCLCPP_INFO(this->get_logger(),"Path[0] = x=%.3f y=%.3f yaw=%.3f",
path_x_[0],path_y_[0],path_yaw_[0]);
// 输出最后一个点
const size_t last = path_x_.size() - 1;
RCLCPP_INFO(this->get_logger(),"Path[last] = x=%.3f y=%.3f yaw=%.3f",
path_x_[last],path_y_[last],path_yaw_[last]);
return true;
}
catch (const json::exception &e)
{
RCLCPP_ERROR(this->get_logger(), "JSON parse error: %s", e.what());
return false;
}
catch (const std::exception &e)
{
RCLCPP_ERROR(this->get_logger(),"Exception: %s",e.what());
return false;
}
}
void PathVisualizer::publishVehiclePoseRviz()
{
visualization_msgs::msg::Marker marker;
marker.header.frame_id="odom";
marker.header.stamp=this->now();
marker.ns="vehicle";
marker.id=0;
// 箭头
marker.type= visualization_msgs::msg::Marker::ARROW;
marker.action= visualization_msgs::msg::Marker::ADD;
// 箭头大小
marker.scale.x=1.5; // 长度
marker.scale.y=0.1; // 宽度
marker.scale.z=0.08;
// 颜色
marker.color.a=1.0;
marker.color.r=0.9;
marker.color.g=0.4;
marker.color.b=0.1;
// 位置
marker.pose.position.x=x_;
marker.pose.position.y=y_;
marker.pose.position.z=0.4;
// yaw转四元数
double cy = cos(yaw_/2.0);
double sy = sin(yaw_/2.0);
marker.pose.orientation.x=0;
marker.pose.orientation.y=0;
marker.pose.orientation.z=sy;
marker.pose.orientation.w=cy;
vehicle_pub_->publish(marker);
// 2. vehicle cube
// visualization_msgs::msg::Marker body;
// body.header.frame_id = "odom";
// body.header.stamp = this->now();
// body.ns = "vehicle";
// body.id = 1;
// body.type = visualization_msgs::msg::Marker::CUBE;
// body.action = visualization_msgs::msg::Marker::ADD;
// // 车辆尺寸
// body.scale.x = 0.1; // 车长
// body.scale.y = 0.1; // 车宽
// body.scale.z = 0.3; // 高度
// body.color.a = 0.8;
// body.color.r = 0.0;
// body.color.g = 0.0;
// body.color.b = 1.0;
// // 位置
// body.pose.position.x = x_;
// body.pose.position.y = y_;
// body.pose.position.z = 0.1;
// // 姿态
// body.pose.orientation.x = 0;
// body.pose.orientation.y = 0;
// body.pose.orientation.z = sy;
// body.pose.orientation.w = cy;
// vehicle_pub_->publish(body);
}
void PathVisualizer::publishVehicleInfoRviz(){
visualization_msgs::msg::Marker text;
text.header.stamp = this->get_clock()->now();
text.header.frame_id = "base_link";
text.ns = "vehicle_info";
text.id = 0;
text.type =
visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
text.action =
visualization_msgs::msg::Marker::ADD;
text.pose.position.x = 0.0;
text.pose.position.y = 0.0;
text.pose.position.z = 2.0;
text.color.a = 0.6;
text.color.r = 1.0;
text.color.g = 1.0;
text.color.b = 1.0;
text.scale.z = 0.15;
text.text =
"X: " + std::to_string(x_) + "\n" +
"Y: " + std::to_string(y_) + "\n" +
"Yaw: " + std::to_string(yaw_);
vehicle_info_pub_->publish(text);
}
void PathVisualizer::publishPathRviz()
{
visualization_msgs::msg::Marker path;
path.header.frame_id="odom";
path.header.stamp=this->now();
path.ns="path";
path.id=0;
path.type=visualization_msgs::msg::Marker::SPHERE_LIST;
path.action=visualization_msgs::msg::Marker::ADD;
path.scale.x = 0.15;
path.scale.y = 0.15;
path.scale.z = 0.15;
path.color.a=1.0;
path.color.r=1.0;
path.color.g=0.3;
path.color.b=0.05;
double cos_yaw = std::cos(yaw_);
double sin_yaw = std::sin(yaw_);
for(size_t i=0;i<path_x_.size();i++)
{
geometry_msgs::msg::Point p;
p.x=path_x_[i];
p.y=path_y_[i];
p.z=0.05;
path.points.push_back(p);
}
path_pub_->publish(path);
}
void PathVisualizer::publishHistoryPathRviz(){
history_x_.push_back(x_);
history_y_.push_back(y_);
history_yaw_.push_back(yaw_);
visualization_msgs::msg::Marker history_pose;
history_pose.header.frame_id="odom";
history_pose.header.stamp=this->now();
history_pose.id=0;
history_pose.type=visualization_msgs::msg::Marker::LINE_STRIP;
history_pose.action=visualization_msgs::msg::Marker::ADD;
history_pose.scale.x = 0.10;
history_pose.scale.y = 0.10;
history_pose.scale.z = 0.10;
history_pose.color.a=1.0;
history_pose.color.r=0.05;
history_pose.color.g=0.05;
history_pose.color.b=1.0;
double cos_yaw = std::cos(yaw_);
double sin_yaw = std::sin(yaw_);
for(size_t i=0;i<history_x_.size();i++)
{
geometry_msgs::msg::Point p;
p.x=history_x_[i];
p.y=history_y_[i];
p.z=0.10;
history_pose.points.push_back(p);
}
history_pose_pub_->publish(history_pose);
}
void PathVisualizer::localozation_callback(const geometry_msgs::msg::Pose::SharedPtr msg)
{
double lat =msg->position.x;
double lon =msg->position.y;
if(!origin_set_)
{
origin_lat_=lat;
origin_lon_=lon;
origin_yaw_ = quaternion_to_yaw(msg->orientation.x,
msg->orientation.y,
msg->orientation.z,
msg->orientation.w);
origin_set_=true;
RCLCPP_INFO(this->get_logger(),"GPS origin set lat: %f lon: %f",
origin_lat_,origin_lon_);
}
// 转换局部坐标
x_=gps_to_x(lat,lon);
y_=gps_to_y(lat,lon);
yaw_=quaternion_to_yaw(msg->orientation.x,
msg->orientation.y,
msg->orientation.z,
msg->orientation.w
);
has_pose_=true;
// 发布 TF 与 path 可视化
publishVehicleTFRviz();
publishHistoryPathRviz();
if(origin_set_ && !path_transed){
transformPathToVehicleStart();
path_transed=true;
RCLCPP_INFO(
this->get_logger(),
"Path transformation completed.");
densifyPath(0.2);
publishPathRviz();
publishOdomPath();
}
}
void PathVisualizer::control()
{
if(!has_pose_)
return;
publishVehiclePoseRviz();
publishVehicleInfoRviz();
publishLocalPose();
target_index_= findClosestPoint(forward_index_limit_);
int target =findLookAheadPoint();
double target_x =path_x_[target];
double target_y =path_y_[target];
double target_yaw =path_yaw_[target];
/*世界坐标转换到车辆坐标系*/
double dx =target_x-x_;
double dy =target_y-y_;
double local_x =cos(yaw_)*dx+sin(yaw_)*dy;
double local_y =-sin(yaw_)*dx+cos(yaw_)*dy;
/*Pure Pursuit*/
double Ld =sqrt(local_x*local_x+local_y*local_y);
double curvature =2.0*local_y/(Ld*Ld);
geometry_msgs::msg::Twist cmd;
double speed=max_speed_;
cmd.linear.x =speed;
cmd.angular.z =speed*curvature;
if(cmd.angular.z >max_yaw_rate_)
{
cmd.angular.z=max_yaw_rate_;
}
if(cmd.angular.z <-max_yaw_rate_)
{
cmd.angular.z=-max_yaw_rate_;
}
cmd_pub_->publish(cmd);
RCLCPP_INFO_THROTTLE(
this->get_logger(),
*this->get_clock(),
1000,
"\n"
"========== Pure Pursuit ==========\n"
"Robot:\n"
" x=%.3f y=%.3f yaw=%.3f\n"
"Target:\n"
" index=%d\n"
" x=%.3f y=%.3f\n,yaw=%.3f\n"
"World error:\n"
" dx=%.3f dy=%.3f\n"
"Vehicle frame:\n"
" local_x=%.3f local_y=%.3f\n"
"PP:\n"
" LookAhead=%.3f\n"
" curvature=%.5f\n"
"Command:\n"
" v=%.3f w=%.3f",
x_,
y_,
yaw_,
target,
target_x,
target_y,
target_yaw,
dx,
dy,
local_x,
local_y,
Ld,
curvature,
cmd.linear.x,
cmd.angular.z
);
}
double PathVisualizer::normalize_angle(double angle)
{
while(angle>M_PI)
angle-=2*M_PI;
while(angle<-M_PI)
angle+=2*M_PI;
return angle;
}
double PathVisualizer::quaternion_to_yaw(double x,double y,double z,double w)
{
double siny =2*(w*z+x*y);
double cosy = 1-2*(y*y+z*z);
return atan2(siny,cosy);
}
double PathVisualizer::gps_to_x(double lat,double lon)
{
double R=6378137.0;
double dlon =(lon-origin_lon_)*M_PI/180.0;
double lat_rad =origin_lat_*M_PI/180.0;
return R*cos(lat_rad)*dlon;
}
double PathVisualizer::gps_to_y(double lat,double lon)
{
double R=6378137.0;
double dlat =(lat-origin_lat_)*M_PI/180.0;
return R*dlat;
}
int PathVisualizer::findLookAheadPoint()
{
for(size_t i=target_index_;i<path_x_.size();i++)
{
double dx = path_x_[i]-x_;
double dy = path_y_[i]-y_;
double dist = sqrt(dx*dx+dy*dy);
if(dist >= lookahead_distance_)
{
return i;
}
}
return path_x_.size()-1;
}
int PathVisualizer::findClosestPoint(int forward_index_limit)
{
double min_dist=1e9;
int index=target_index_;
for(int i=target_index_;i<path_x_.size()
&& i<target_index_+forward_index_limit;i++)
{
double dx=path_x_[i]-x_;
double dy=path_y_[i]-y_;
double d=sqrt(dx*dx+dy*dy);
if(d<min_dist)
{
min_dist=d;
index=i;
}
}
return index;
}
void PathVisualizer::publishLocalPose()
{
geometry_msgs::msg::Pose local_pose;
local_pose.position.x = x_;
local_pose.position.y = y_;
tf2::Quaternion orientation;
orientation.setRPY(0, 0, yaw_);
local_pose.orientation.x = orientation.x();
local_pose.orientation.y = orientation.y();
local_pose.orientation.z = orientation.z();
local_pose.orientation.w = orientation.w();
local_pose_pub_->publish(local_pose);
}
void PathVisualizer::publishVehicleTFRviz()
{
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = this->now();
// 父坐标系
transform.header.frame_id = "odom";
// 子坐标系
transform.child_frame_id = "base_link";
// 位置
transform.transform.translation.x = x_;
transform.transform.translation.y = y_;
transform.transform.translation.z = 0.38;
// yaw -> quaternion
tf2::Quaternion q;
q.setRPY(
0.0,
0.0,
yaw_
);
transform.transform.rotation.x = q.x();
transform.transform.rotation.y = q.y();
transform.transform.rotation.z = q.z();
transform.transform.rotation.w = q.w();
tf_broadcaster_->sendTransform(transform);
}
void PathVisualizer::densifyPath(double interval)
{
if (path_x_.size() < 2 ||
path_x_.size() != path_y_.size() ||
path_x_.size() != path_yaw_.size() ||
interval <= 0.0)
{
return;
}
// 保存原始路径
const auto old_x = path_x_;
const auto old_y = path_y_;
const auto old_yaw = path_yaw_;
// 计算累计弧长
std::vector<double> s(old_x.size(), 0.0);
for (size_t i = 1; i < old_x.size(); ++i)
{
double dx = old_x[i] - old_x[i - 1];
double dy = old_y[i] - old_y[i - 1];
s[i] = s[i - 1] + std::hypot(dx, dy);
}
const double total_length = s.back();
if (total_length < 1e-6)
return;
// 清空原路径，重新生成
path_x_.clear();
path_y_.clear();
path_yaw_.clear();
size_t segment = 0;
for (double target_s = 0.0;
target_s < total_length;
target_s += interval)
{
// 找到 target_s 所在的原始路径段
while (segment + 1 < s.size() &&
s[segment + 1] < target_s)
{
++segment;
}
if (segment + 1 >= s.size())
break;
double segment_length = s[segment + 1] - s[segment];
double ratio = 0.0;
if (segment_length > 1e-6)
{
ratio = (target_s - s[segment]) /
segment_length;
}
// x 插值
double x = old_x[segment] +
ratio * (old_x[segment + 1] -
old_x[segment]);
// y 插值
double y = old_y[segment] +
ratio * (old_y[segment + 1] -
old_y[segment]);
// yaw 插值
double yaw1 = old_yaw[segment];
double yaw2 = old_yaw[segment + 1];
double dyaw = yaw2 - yaw1;
// 处理 yaw 的 ±PI 跳变
while (dyaw > M_PI)
dyaw -= 2.0 * M_PI;
while (dyaw < -M_PI)
dyaw += 2.0 * M_PI;
double yaw = yaw1 + ratio * dyaw;
// 归一化到 [-PI, PI]
while (yaw > M_PI)
yaw -= 2.0 * M_PI;
while (yaw < -M_PI)
yaw += 2.0 * M_PI;
path_x_.push_back(x);
path_y_.push_back(y);
path_yaw_.push_back(yaw);
}
// 确保最后一个原始点保留
path_x_.push_back(old_x.back());
path_y_.push_back(old_y.back());
path_yaw_.push_back(old_yaw.back());
}
void PathVisualizer::publishOdomPath(){
nav_msgs::msg::Path path_msg;
path_msg.header.stamp = this->now();
path_msg.header.frame_id = "odom";
path_msg.poses.reserve(path_x_.size());
for (size_t i = 0; i <path_x_.size(); ++i) {
geometry_msgs::msg::PoseStamped pose;
pose.header.stamp = path_msg.header.stamp;
pose.header.frame_id = "odom";
pose.pose.position.x = path_x_[i];
pose.pose.position.y = path_y_[i];
pose.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, path_yaw_[i]);
pose.pose.orientation.x = q.x();
pose.pose.orientation.y = q.y();
pose.pose.orientation.z = q.z();
pose.pose.orientation.w = q.w();
path_msg.poses.push_back(pose);
}
odom_path_pub_->publish(path_msg);
}
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
std::string file_name;
std::cout << "请输入路径文件路径: ";
std::getline(std::cin, file_name);
if (file_name.empty())
{
std::cerr << "错误：路径文件不能为空！" << std::endl;
rclcpp::shutdown();
return 1;
}
std::cout << "正在加载路径文件: " << file_name << std::endl;
auto pathvisualizer =
std::make_shared<PathVisualizer>(file_name);
rclcpp::spin(pathvisualizer);
rclcpp::shutdown();
return 0;
}

258
src/WebotsRobotDriver.cpp
View File

@ -1,258 +0,0 @@
#include "sweet_webots_robot/WebotsRobotDriver.hpp"
#include "rclcpp/rclcpp.hpp"
#include <cstdio>
#include <functional>
#include <webots/motor.h>
#include <webots/robot.h>
#include <webots/gps.h>
#include <webots/inertial_unit.h>
#include <webots/compass.h>
#include <webots/speaker.h>
#define HALF_DISTANCE_BETWEEN_WHEELS 0.045
#define WHEEL_RADIUS 0.025
#define TIME_PERID 100
#define MAX_SPEED 1.0
#define MAX_MOTOR_EX_FACTORY_SPEED 30000
#define MAX_AUTO_SPEED 6000
#define WHEEL_DIS 0.95
#define WHEEL_DIAMETER 0.206
#define MOTOR_RATIO 1.0
#define CENTER_2_HEADER (0.935)
namespace webots_robot_driver {
void WebotsRobotDriver::init(
webots_ros2_driver::WebotsNode *node,
std::unordered_map<std::string, std::string> &parameters) {
speaker = wb_robot_get_device("speaker");
right_motor = wb_robot_get_device("right wheel motor");
left_motor = wb_robot_get_device("left wheel motor");
wb_motor_set_position(left_motor, INFINITY);
wb_motor_set_velocity(left_motor, 0.0);
wb_motor_set_position(right_motor, INFINITY);
wb_motor_set_velocity(right_motor, 0.0);
gp = wb_robot_get_device("gps");
wb_gps_enable(gp, TIME_PERID);
imu = wb_robot_get_device("inertial unit");
wb_inertial_unit_enable(imu, TIME_PERID);
compass = wb_robot_get_device("compass");
wb_compass_enable(compass, TIME_PERID);
cmd_vel_subscription_ = node->create_subscription<geometry_msgs::msg::Twist>(
"/cmd_vel_", rclcpp::SensorDataQoS().reliable(),
std::bind(&WebotsRobotDriver::cmdVelCallback, this, std::placeholders::_1));
twist_cmd_vel_sub = node->create_subscription<geometry_msgs::msg::Twist>\
("twist_cmd_vel", 10, std::bind(&WebotsRobotDriver::twist_callback, this, std::placeholders::_1));
move_base_sub = node->create_subscription<sweet_msg::msg::Movebase>\
("move_base", 10, std::bind(&WebotsRobotDriver::move_base_callback, this, std::placeholders::_1));
rtk_pose_publisher = node->create_publisher<geometry_msgs::msg::Pose>("rtk_pose", 10);
}
void WebotsRobotDriver::cmdVelCallback(
const geometry_msgs::msg::Twist::SharedPtr msg) {
//std::cout<<"[cmdVelCallback] linear: "<<msg->linear<<std::endl;
printf("[cmdVelCallback] linear: %.4lf\n", msg->linear);
//cmd_vel_msg.linear = msg->linear;
//cmd_vel_msg.angular = msg->angular;
}
void WebotsRobotDriver::getRobotCenterPose(double &x, double &y, const float yaw)
{
x = x - cos(yaw) * CENTER_2_HEADER;
y = y - sin(yaw) * CENTER_2_HEADER;
}
void WebotsRobotDriver::twist_callback(const geometry_msgs::msg::Twist::SharedPtr twist_msg)
{
//std::cout<<"*********[twist_callback]*********maxspeed: "<<maxMotorSpeed<<std::endl;
float left_vel = (twist_msg->linear.x - twist_msg->angular.z * 0.5 * WHEEL_DIS) * 60; //r/min
float right_vel = (twist_msg->linear.x + twist_msg->angular.z * 0.5 * WHEEL_DIS) * 60;
int left_motor_rpm = left_vel / (M_PI * WHEEL_DIAMETER) * 10 * 20; // 0.1 r/min
int right_motor_rpm = right_vel / (M_PI * WHEEL_DIAMETER) * 10 * 20;
#ifndef MOVE_BASE_FILTER
static int last_left_motor_rpm = 0.0;
static int last_right_motor_rpm = 0.0;
//std::cout<<"[twist_callback] last_speed:"<<last_speed<<", last_angle:"<<last_angle\
<<", speed:"<<speed_val<<", angle:"<<angle<<std::endl;
if(fabs(left_motor_rpm - last_left_motor_rpm) < 1e-6 && fabs(right_motor_rpm - last_right_motor_rpm) < 1e-6)
{
//std::cout<<"[twist_callback] speed and angle is the same as last time, return!"<<std::endl;
return;
}
else
{
last_left_motor_rpm = left_motor_rpm;
last_right_motor_rpm = right_motor_rpm;
}
#endif
left_motor_rpm = left_motor_rpm > MAX_MOTOR_EX_FACTORY_SPEED ? MAX_MOTOR_EX_FACTORY_SPEED : left_motor_rpm;
right_motor_rpm = right_motor_rpm > MAX_MOTOR_EX_FACTORY_SPEED ? MAX_MOTOR_EX_FACTORY_SPEED : right_motor_rpm;
cmd_vel_msg.linear.x =(float)left_motor_rpm / MAX_MOTOR_EX_FACTORY_SPEED;
cmd_vel_msg.angular.z = -(float)right_motor_rpm / MAX_MOTOR_EX_FACTORY_SPEED;
std::cout<<"[twist_callback] linear_vel:"<<cmd_vel_msg.linear.x<<", angular_vel:"<<cmd_vel_msg.angular.z<<\
", left_speed:"<<left_motor_rpm<<", right_speed:"<<right_motor_rpm<<std::endl;
}
void WebotsRobotDriver::move_base_callback(const sweet_msg::msg::Movebase::SharedPtr move_base_msg)
{
float speed_val = move_base_msg->speed;
float angle = move_base_msg->angle;
#ifndef MOVE_BASE_FILTER
static float last_speed = 0.0;
static float last_angle = 0.0;
//std::cout<<"[move_base_callback] last_speed:"<<last_speed<<", last_angle:"<<last_angle\
<<", speed:"<<speed_val<<", angle:"<<angle<<std::endl;
if (fabs(speed_val - last_speed) < 1e-6 && fabs(angle - last_angle) < 1e-6)
{
// std::cout<<"[move_base_callback] speed and angle is the same as last time, return!"<<std::endl;
return;
}
else
{
last_speed = speed_val;
last_angle = angle;
}
#endif
speed_val = speed_val > MAX_SPEED ? MAX_SPEED : speed_val;
speed_val = speed_val < (-1 * MAX_SPEED) ? (-1 * MAX_SPEED) : speed_val;
// std::cout<<"speed_Val: "<<speed_val<<std::endl;
angle = -2 * sin(1 * angle);
float left_speed, right_speed;
if (0 <= angle)
{
left_speed = (speed_val * MAX_MOTOR_EX_FACTORY_SPEED) * 1;
right_speed = ((-1 + angle) * speed_val * MAX_MOTOR_EX_FACTORY_SPEED) * 1;
}
else
{
left_speed = ((1 + angle) * speed_val * MAX_MOTOR_EX_FACTORY_SPEED) * 1;
right_speed = (-1 * speed_val * MAX_MOTOR_EX_FACTORY_SPEED) * 1;
}
cmd_vel_msg.linear.x = (float)left_speed / MAX_MOTOR_EX_FACTORY_SPEED * 1.5;
cmd_vel_msg.angular.z = (float)right_speed / MAX_MOTOR_EX_FACTORY_SPEED * 1.5;
double speed = cmd_vel_msg.linear.x;
if(speed > 0.01)
{
wb_speaker_play_sound(
speaker,
speaker,
"sounds/motor.wav",
1.0,
1.0,
0.0,
false
);
}
std::cout << "[move_base_callback] angle:" << angle << ", speed:" << speed_val \
<< ", left_speed:" << cmd_vel_msg.linear.x << ", right_speed:" << cmd_vel_msg.angular.z << std::endl;
}
void WebotsRobotDriver::pub_current_pose(const double lat, const double lon, const float roll, const float pitch, const float yaw)
{
geometry_msgs::msg::Pose rtk_pose;
rtk_pose.position.x = lat;
rtk_pose.position.y = lon;
rtk_pose.position.z = yaw;
//getRobotCenterPose(rtk_pose.position.x, rtk_pose.position.y, yaw);
tf2::Quaternion orientation;
orientation.setRPY(roll, pitch, yaw);
rtk_pose.orientation = tf2::toMsg(orientation);
rtk_pose_publisher->publish(rtk_pose);
}
float WebotsRobotDriver::get_bearing_in_degrees() {
const double *north = wb_compass_get_values(compass);
double rad = atan2(north[1], north[0]);
// std::cout<<"rad1:"<<rad<<std::endl;
//rad -= 0.5 * M_PI;
rad = 0.5 * M_PI - rad;
//std::cout<<"rad2:"<<rad<<std::endl;
while(rad >= M_PI) rad -= 2 * M_PI;
while(rad <= -M_PI) rad += 2 * M_PI;
return rad;
}
void WebotsRobotDriver::step() {
auto forward_speed = cmd_vel_msg.linear.x;
auto angular_speed = -cmd_vel_msg.angular.z;
//auto command_motor_left =\
(forward_speed - angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / \
WHEEL_RADIUS;
//auto command_motor_right =\
(forward_speed + angular_speed * HALF_DISTANCE_BETWEEN_WHEELS) / \
WHEEL_RADIUS;
auto command_motor_left = forward_speed;
auto command_motor_right = angular_speed;
wb_motor_set_velocity(left_motor, command_motor_left);
wb_motor_set_velocity(right_motor, command_motor_right);
//std::cout<<"gps x:"<<wb_gps_get_values(gp)[0]<<", y:"<<wb_gps_get_values(gp)[1]<<std::endl;
//std::cout<<"imu yaw:"<<wb_inertial_unit_get_roll_pitch_yaw(imu)[2]<<std::endl;
double gps_lat = wb_gps_get_values(gp)[0];
double gps_lon = wb_gps_get_values(gp)[1];
//float imu_roll = wb_inertial_unit_get_roll_pitch_yaw(imu)[0];
//float imu_pitch = wb_inertial_unit_get_roll_pitch_yaw(imu)[1];
//float imu_yaw = wb_inertial_unit_get_roll_pitch_yaw(imu)[2];
float bearing = get_bearing_in_degrees();
//std::cout<<"bearing: "<<bearing<<std::endl;
// std::cout <<"left_speed:" << forward_speed << ", right_speed:" << angular_speed << std::endl;
pub_current_pose(gps_lat, gps_lon, 0, 0, bearing);
}
} // namespace webots_robot_driver
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(webots_robot_driver::WebotsRobotDriver,
webots_ros2_driver::PluginInterface)
/*int main(int argc, char ** argv)
{
(void) argc;
(void) argv;
printf("hello world sweet_webots_robot package\n");
return 0;
}*/

199
urdf/SweetAiRviz.rviz
View File

@ -1,199 +0,0 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 1522
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/Marker
Enabled: true
Name: PlanningPath
Namespaces:
path: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planned_path
Value: true
- Class: rviz_default_plugins/Marker
Enabled: true
Name: VehiclePose
Namespaces:
vehicle: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /vehicle_pose
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MarkerArray
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /vehicle_pose_array
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: true
Description File: /home/sweetai/webots_simulation/src/sweet_webots_robot/urdf/rviz_robot.urdf
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: SweetRobot
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz_default_plugins/Marker
Enabled: true
Name: HistoryPosePath
Namespaces:
"": true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /history_pose
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 48.464454650878906
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 13.90898609161377
Y: 3.2684290409088135
Z: -1.6824548244476318
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.3847966194152832
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 2.095363140106201
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1926
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000039c000006eafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006ea0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000035c000002f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e000002f20000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000008d8000006ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 3200
X: 0
Y: 0

19
urdf/rviz_robot.urdf
View File

@ -1,19 +0,0 @@
<?xml version="1.0"?>
<robot name="tracked_robot">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh
filename="package://sweet_webots_robot/meshes/tracked_robot.obj"
scale="1 1 1"/>
</geometry>
</visual>
</link>
</robot>

9
webots_robot_driver.xml
View File

@ -1,9 +0,0 @@
<library path="sweet_webots_robot">
<!-- The `type` attribute is a reference to the plugin class. -->
<!-- The `base_class_type` attribute is always `webots_ros2_driver::PluginInterface`. -->
<class type="webots_robot_driver::WebotsRobotDriver" base_class_type="webots_ros2_driver::PluginInterface">
<description>
This is a Webots ROS 2 plugin about sweet robot
</description>
</class>
</library>

10
worlds/.sweet_robot.wbproj
View File

@ -1,10 +0,0 @@
Webots Project File version R2023a
perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a00000266fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002660000003c00ffffff000000030000073800000160fc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007380000005400ffffff000004f80000026600000004000000040000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000006010000000101
sceneTreePerspectives: 000000ff000000010000000300000016000002b1000000fc0100000006010000000201
maximizedDockId: -1
centralWidgetVisible: 1
orthographicViewHeight: 1
textFiles: -1
consoles: Console:All:All
renderingDevicePerspectives: tracked_robot:camera;0;1;0.831485;0.266667

BIN
worlds/grass.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 103 KiB

152
worlds/my_world.wbt
View File

@ -1,152 +0,0 @@
#VRML_SIM R2022b utf8
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto"
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/CircleArena.proto"
WorldInfo {
}
Viewpoint {
orientation -0.33185733874619844 -0.09874274160469809 0.9381474178937331 3.686018050088086
position 1.700313773507203 1.0549607538959629 1.4846240848267684
follow "my_robot"
}
TexturedBackground {
}
TexturedBackgroundLight {
}
CircleArena {
}
Robot {
children [
HingeJoint {
jointParameters HingeJointParameters {
axis 0 1 0
anchor 0 0 0.025
}
device [
RotationalMotor {
name "left wheel motor"
}
]
endPoint Solid {
translation 0 0.045 0.025
children [
DEF WHEEL Transform {
rotation 1 0 0 1.5707996938995747
children [
Shape {
appearance PBRAppearance {
baseColor 1 0 0
roughness 1
metalness 0
}
geometry Cylinder {
height 0.01
radius 0.025
}
}
]
}
]
name "left wheel"
boundingObject USE WHEEL
physics Physics {
}
}
}
HingeJoint {
jointParameters HingeJointParameters {
axis 0 1 0
anchor 0 0 0.025
}
device [
RotationalMotor {
name "right wheel motor"
}
]
endPoint Solid {
translation 0 -0.045 0.025
children [
USE WHEEL
]
name "right wheel"
boundingObject USE WHEEL
physics Physics {
}
}
}
Transform {
translation 0 0 0.0415
children [
Shape {
appearance PBRAppearance {
baseColor 0 0 1
roughness 1
metalness 0
}
geometry DEF BODY Cylinder {
height 0.08
radius 0.045
}
}
]
}
DistanceSensor {
translation 0.042 0.02 0.063
rotation 0 0 1 0.5236003061004253
children [
DEF SENSOR Transform {
rotation 0 1 0 1.5708
children [
Shape {
appearance PBRAppearance {
baseColor 1 1 0
roughness 1
metalness 0
}
geometry Cylinder {
height 0.004
radius 0.008
}
}
]
}
]
name "ds0"
lookupTable [
0 1020 0
0.05 1020 0
0.15 0 0
]
numberOfRays 2
aperture 1
}
DistanceSensor {
translation 0.042 -0.02 0.063
rotation 0 0 1 -0.5235996938995747
children [
USE SENSOR
]
name "ds1"
lookupTable [
0 1020 0
0.05 1020 0
0.15 0 0
]
numberOfRays 2
aperture 1
}
]
boundingObject Transform {
translation 0 0 0.0415
children [
USE BODY
]
}
physics Physics {
}
controller "<extern>"
name "my_robot"
}

875
worlds/sweet_robot.wbt
View File

File diff suppressed because one or more lines are too long