mirror of
https://github.com/westonrobot/scout_ros.git
synced 2026-01-12 11:20:02 +08:00
initial release of webots simulation for scout
This commit is contained in:
Ruixiang Du
129
scout_webots_sim/src/scout_webots_interface.cpp
Normal file
129
scout_webots_sim/src/scout_webots_interface.cpp
Normal 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
119
scout_webots_sim/src/scout_webots_node.cpp
Normal 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;
|
||||
}
|
||||
Reference in New Issue
Block a user