mirrors/scout_ros
1
0
Fork 0
mirror of https://github.com/westonrobot/scout_ros.git synced 2026-01-12 03:15:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

initial release of webots simulation for scout

Browse Source
This commit is contained in:
Ruixiang Du
2020-01-31 14:28:38 +08:00
parent 16542b008f
commit 7d4993b1e7
13 changed files with 1313 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
README.md
View File

@@ -78,7 +78,23 @@ Replace "<username>" in the above command with your Linux username. You need to
$ catkin_make
```
3. Launch ROS nodes
3. Setup Webots simulation
* Install Webots R2020a-rev1 (download from https://cyberbotics.com/ )
* Install Webots ROS package
```
$ sudo apt install ros-melodic-webots-ros
```
* Set WEBOTS_HOME variable, add the following line to your "~/.bashrc"
```
export WEBOTS_HOME=/usr/local/webots
```
4. Launch ROS nodes
* Start the base node
@@ -92,6 +108,12 @@ Replace "<username>" in the above command with your Linux username. You need to
$ roslaunch scout_bringup scout_minimal_uart.launch
```
* Or you can start the Webots-based simulation
```
$ roslaunch scout_bringup scout_base_sim.launch
```
* Start the keyboard tele-op node
```

3
scout_bringup/launch/scout_base_sim.launch Normal file
View File

@@ -0,0 +1,3 @@
<launch>
<include file="$(find scout_webots_sim)/launch/scout_base.launch" />
</launch>

219
scout_webots_sim/CMakeLists.txt Normal file
View File

@@ -0,0 +1,219 @@
cmake_minimum_required(VERSION 2.8.3)
project(scout_webots_sim)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roslaunch
geometry_msgs
roscpp
rospy
scout_base
sensor_msgs
std_msgs
message_generation
tf
webots_ros
pcl_ros
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# BoolStamped.msg
# Float64Stamped.msg
# Int32Stamped.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# camera_get_focus_info.srv
# camera_get_info.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# sensor_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES scout_webots_sim
CATKIN_DEPENDS geometry_msgs roscpp rospy scout_base sensor_msgs std_msgs message_runtime tf webots_ros pcl_ros
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# set(SCOUT_WEBOTS_SRC
# src/scout_webots_interface.cpp
# )
# add_library(scout_webots_sim STATIC ${SCOUT_WEBOTS_SRC})
# target_link_libraries(scout_webots_sim ${catkin_LIBRARIES})
# target_include_directories(scout_webots_sim PUBLIC
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>
# PRIVATE src)
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
## Declare a C++ executable
add_executable(scout_webots_node src/scout_webots_node.cpp src/scout_webots_interface.cpp)
add_dependencies(scout_webots_node webots_ros_generate_messages_cpp)
target_link_libraries(scout_webots_node ${catkin_LIBRARIES})
#############
## Install ##
#############
roslaunch_add_file_check(launch)
install(TARGETS scout_webots_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
install(DIRECTORY launch urdf worlds
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_scout_webots_sim.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

15
scout_webots_sim/README.md Normal file
View File

@@ -0,0 +1,15 @@
## scout_webots_sim
* Install dependencies
```
$ sudo apt install ros-melodic-webots-ros
```
* Set WEBOTS_HOME variable
Add the following line to your "~/.bashrc"
```
export WEBOTS_HOME=/usr/local/webots
```

36
scout_webots_sim/include/scout_webots_sim/scout_sim_params.hpp Normal file
View File

@@ -0,0 +1,36 @@
/*
* scout_sim_params.hpp
*
* Created on: Sep 27, 2019 15:08
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_SIM_PARAMS_HPP
#define SCOUT_SIM_PARAMS_HPP
#include <cstdint>
namespace wescore
{
struct ScoutSimParams
{
/* Scout Parameters */
static constexpr double max_steer_angle = 30.0; // in degree
static constexpr double track = 0.576; // in meter (left & right wheel distance)
static constexpr double wheelbase = 0.648; // in meter (front & rear wheel distance)
static constexpr double wheel_radius = 0.165; // in meter
// from user manual v1.2.8 P18
// max linear velocity: 1.5 m/s
// max angular velocity: 0.7853 rad/s
static constexpr double max_linear_speed = 1.5; // in m/s
static constexpr double max_angular_speed = 0.7853; // in rad/s
static constexpr double max_speed_cmd = 10.0; // in rad/s
};
} // namespace wescore
#endif /* SCOUT_SIM_PARAMS_HPP */

41
scout_webots_sim/include/scout_webots_sim/scout_webots_interface.hpp Normal file
View File

@@ -0,0 +1,41 @@
/*
* scout_webots_interface.hpp
*
* Created on: Sep 26, 2019 23:04
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#ifndef SCOUT_WEBOTS_INTERFACE_HPP
#define SCOUT_WEBOTS_INTERFACE_HPP
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/Imu.h>
#include "scout_base/scout_messenger.hpp"
namespace wescore
{
class ScoutWebotsInterface
{
public:
ScoutWebotsInterface(ros::NodeHandle *nh, ScoutROSMessenger* msger, uint32_t time_step);
void InitComponents(std::string controller_name);
void UpdateSimState();
private:
ros::NodeHandle *nh_;
ScoutROSMessenger* messenger_;
uint32_t time_step_;
std::string robot_name_ = "agilex_scout";
const std::vector<std::string> motor_names_{"motor_fr", "motor_fl", "motor_rl", "motor_rr"};
};
} // namespace wescore
#endif /* SCOUT_WEBOTS_INTERFACE_HPP */

23
scout_webots_sim/launch/scout_base.launch Normal file
View File

@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<launch>
<!-- start Webots -->
<arg name="no-gui" default="false," doc="Start Webots with minimal GUI"/>
<include file="$(find scout_webots_sim)/launch/webots.launch">
<arg name="mode" value="realtime"/>
<arg name="no-gui" value="$(arg no-gui)"/>
<arg name="world" value="$(find scout_webots_sim)/worlds/scout_base.wbt"/>
</include>
<arg name="auto-close" default="false" doc="Startup mode"/>
<node name="scout_webots_node" pkg="scout_webots_sim" type="scout_webots_node" required="$(arg auto-close)">
<param name="use_sim_time" value="true" />
<param name="odom_frame" type="string" value="odom" />
<param name="base_frame" type="string" value="base_link" />
<param name="simulated_robot" type="bool" value="true" />
<param name="sim_control_rate" value="50" />
</node>
<param name="robot_description" textfile="$(find scout_webots_sim)/urdf/agilex_scout_webots.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>

7
scout_webots_sim/launch/webots.launch Normal file
View File

@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<launch>
<arg name="world" default="" doc="Path to the world to load"/>
<arg name="mode" default="realtime," doc="Startup mode"/>
<arg name="no-gui" default="false," doc="Start Webots with minimal GUI"/>
<node name="webots" pkg="webots_ros" type="webots_launcher.py" args="--world=$(arg world) --mode=$(arg mode) --no-gui=$(arg no-gui)" required="true"/>
</launch>

85
scout_webots_sim/package.xml Normal file
View File

@@ -0,0 +1,85 @@
<?xml version="1.0"?>
<package format="2">
<name>scout_webots_sim</name>
<version>0.0.0</version>
<description>The scout_webots_sim package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="rdu@todo.todo">rdu</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/scout_webots_sim</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>scout_base</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>message_runtime</build_depend>
<build_depend>webots_ros</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>roslaunch</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>scout_base</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>scout_base</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>webots_ros</exec_depend>
<exec_depend>pcl_ros</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

129
scout_webots_sim/src/scout_webots_interface.cpp Normal file
View File

@@ -0,0 +1,129 @@
/*
* scout_webots_interface.cpp
*
* Created on: Sep 26, 2019 23:19
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "scout_webots_sim/scout_webots_interface.hpp"
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_ros/transforms.h>
#include <webots_ros/set_float.h>
#include <webots_ros/get_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/set_bool.h>
#include "scout_webots_sim/scout_sim_params.hpp"
namespace wescore
{
ScoutWebotsInterface::ScoutWebotsInterface(ros::NodeHandle *nh, ScoutROSMessenger *msger, uint32_t time_step)
: nh_(nh), messenger_(msger), time_step_(time_step)
{
}
void ScoutWebotsInterface::InitComponents(std::string controller_name)
{
// reset controller name
robot_name_ = controller_name;
// init motors
for (int i = 0; i < 4; ++i)
{
// position
webots_ros::set_float set_position_srv;
ros::ServiceClient set_position_client = nh_->serviceClient<webots_ros::set_float>(robot_name_ + "/" + std::string(motor_names_[i]) +
std::string("/set_position"));
set_position_srv.request.value = INFINITY;
if (set_position_client.call(set_position_srv) && set_position_srv.response.success)
ROS_INFO("Position set to INFINITY for motor %s.", motor_names_[i].c_str());
else
ROS_ERROR("Failed to call service set_position on motor %s.", motor_names_[i].c_str());
// speed
ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;
set_velocity_client = nh_->serviceClient<webots_ros::set_float>(robot_name_ + "/" + std::string(motor_names_[i]) +
std::string("/set_velocity"));
set_velocity_srv.request.value = 0.0;
if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)
ROS_INFO("Velocity set to 0.0 for motor %s.", motor_names_[i].c_str());
else
ROS_ERROR("Failed to call service set_velocity on motor %s.", motor_names_[i].c_str());
}
}
void ScoutWebotsInterface::UpdateSimState()
{
// constants for calculation
constexpr double rotation_radius = std::hypot(ScoutSimParams::wheelbase / 2.0, ScoutSimParams::track / 2.0) * 2.0;
constexpr double rotation_theta = std::atan2(ScoutSimParams::wheelbase, ScoutSimParams::track);
// update robot state
double wheel_speeds[4];
for (int i = 0; i < 4; ++i)
{
webots_ros::get_float get_velocity_srv;
ros::ServiceClient get_velocity_client = nh_->serviceClient<webots_ros::get_float>(robot_name_ + "/" + std::string(motor_names_[i]) +
std::string("/get_velocity"));
if (get_velocity_client.call(get_velocity_srv))
{
wheel_speeds[i] = get_velocity_srv.response.value;
ROS_INFO("Velocity set to 0.0 for motor %s.", motor_names_[i].c_str());
}
else
ROS_ERROR("Failed to call service set_velocity on motor %s.", motor_names_[i].c_str());
}
float left_speed = (wheel_speeds[1] + wheel_speeds[2]) / 2.0 * ScoutSimParams::wheel_radius;
float right_speed = (wheel_speeds[0] + wheel_speeds[3]) / 2.0 * ScoutSimParams::wheel_radius;
double linear_speed = (right_speed + left_speed) / 2.0;
double angular_speed = (right_speed - left_speed) * std::cos(rotation_theta) / rotation_radius;
messenger_->PublishSimStateToROS(linear_speed, angular_speed);
// send robot command
double linear, angular;
messenger_->GetCurrentMotionCmdForSim(linear, angular);
if (linear > ScoutSimParams::max_linear_speed)
linear = ScoutSimParams::max_linear_speed;
if (linear < -ScoutSimParams::max_linear_speed)
linear = -ScoutSimParams::max_linear_speed;
if (angular > ScoutSimParams::max_angular_speed)
angular = ScoutSimParams::max_angular_speed;
if (angular < -ScoutSimParams::max_angular_speed)
angular = -ScoutSimParams::max_angular_speed;
double vel_left_cmd = (linear - angular * rotation_radius / std::cos(rotation_theta)) / ScoutSimParams::wheel_radius;
double vel_right_cmd = (linear + angular * rotation_radius / std::cos(rotation_theta)) / ScoutSimParams::wheel_radius;
double wheel_cmds[4];
wheel_cmds[0] = vel_right_cmd;
wheel_cmds[1] = vel_left_cmd;
wheel_cmds[2] = vel_left_cmd;
wheel_cmds[3] = vel_right_cmd;
for (int i = 0; i < 4; ++i)
{
ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;
set_velocity_client = nh_->serviceClient<webots_ros::set_float>(robot_name_ + "/" + std::string(motor_names_[i]) +
std::string("/set_velocity"));
set_velocity_srv.request.value = wheel_cmds[i];
if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)
ROS_INFO("Velocity set to 0.0 for motor %s.", motor_names_[i].c_str());
else
ROS_ERROR("Failed to call service set_velocity on motor %s.", motor_names_[i].c_str());
}
}
} // namespace wescore

119
scout_webots_sim/src/scout_webots_node.cpp Normal file
View File

@@ -0,0 +1,119 @@
/*
* scout_webots_node.cpp
*
* Created on: Sep 26, 2019 23:03
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include <signal.h>
#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include "scout_webots_sim/scout_webots_interface.hpp"
using namespace wescore;
ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;
static int controllerCount;
static std::vector<std::string> controllerList;
void quit(int sig)
{
ROS_INFO("User stopped the 'agilex_scout' node.");
timeStepSrv.request.value = 0;
timeStepClient.call(timeStepSrv);
ros::shutdown();
exit(0);
}
// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name)
{
controllerCount++;
controllerList.push_back(name->data);
ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "scout_webots_node", ros::init_options::AnonymousName);
ros::NodeHandle nh, private_node("~");
ScoutROSMessenger messenger(&nh);
private_node.param<std::string>("odom_frame", messenger.odom_frame_, std::string("odom"));
private_node.param<std::string>("base_frame", messenger.base_frame_, std::string("base_link"));
private_node.param<int>("sim_control_rate", messenger.sim_control_rate_, 50);
private_node.param<bool>("simulated_robot", messenger.simulated_robot_, true);
messenger.SetupSubscription();
const uint32_t time_step = 1000 / messenger.sim_control_rate_;
ScoutWebotsInterface scout_webots(&nh, &messenger, time_step);
signal(SIGINT, quit);
// subscribe to the topic model_name to get the list of availables controllers
std::string controllerName;
ros::Subscriber nameSub = nh.subscribe("model_name", 100, controllerNameCallback);
while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers())
{
ros::spinOnce();
ros::spinOnce();
ros::spinOnce();
}
ros::spinOnce();
// if there is more than one controller available, it let the user choose
if (controllerCount == 1)
controllerName = controllerList[0];
else
{
int wantedController = 0;
std::cout << "Choose the # of the controller you want to use:\n";
std::cin >> wantedController;
if (1 <= wantedController && wantedController <= controllerCount)
controllerName = controllerList[wantedController - 1];
else
{
ROS_ERROR("Invalid number for controller choice.");
return 1;
}
}
ROS_INFO("Using controller: '%s'", controllerName.c_str());
// leave topic once it is not necessary anymore
nameSub.shutdown();
// init robot components
scout_webots.InitComponents(controllerName);
ROS_INFO("Entering ROS main loop...");
// main loop
timeStepClient = nh.serviceClient<webots_ros::set_int>(controllerName + "/robot/time_step");
timeStepSrv.request.value = time_step;
while (ros::ok())
{
if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
{
ROS_ERROR("Failed to call service time_step for next step.");
break;
}
scout_webots.UpdateSimState();
ros::spinOnce();
}
timeStepSrv.request.value = 0;
timeStepClient.call(timeStepSrv);
ros::shutdown();
return 0;
}

72
scout_webots_sim/urdf/agilex_scout_webots.urdf Normal file
View File

@@ -0,0 +1,72 @@
<!--
Copyright 2016 The Cartographer Authors
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.
Reference:
[1] https://github.com/IntelRealSense/librealsense/blob/master/doc/d435i.md
-->
<robot name="agilex_scout_rs16">
<material name="orange">
<color rgba="1.0 0.5 0.2 1" />
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1" />
</material>
<!-- Robosense16 -->
<link name="rslidar">
<visual>
<origin xyz="0.0 0.0 0.0" />
<geometry>
<cylinder length="0.07" radius="0.05" />
</geometry>
<material name="orange" />
</visual>
</link>
<!-- Realsense D435i -->
<link name="imu_link">
<visual>
<origin xyz="0.0 0.0 0.0" />
<geometry>
<box size="0.06 0.04 0.02" />
</geometry>
<material name="orange" />
</visual>
</link>
<link name="base_link" />
<joint name="rslidar_joint" type="fixed">
<parent link="base_link" />
<child link="rslidar" />
<origin xyz="-0.11 0.0 0.71" rpy="0 0 0" />
</joint>
<joint name="imu_joint" type="fixed">
<parent link="base_link" />
<child link="imu_link" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<!--
<joint name="imu_joint" type="fixed">
<parent link="base_link" />
<child link="camera_imu_optical_frame" />
<origin xyz="0 0 0.0" rpy="-1.57 0 -1.57" />
</joint>
-->
</robot>

541
scout_webots_sim/worlds/scout_base.wbt Normal file
View File

File diff suppressed because one or more lines are too long