initial release of webots simulation for scout

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

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 */

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 */