merged with scout_sim

scout_webots_sim/CMakeLists.txt

@@ -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)

scout_webots_sim/include/scout_webots_sim/scout_sim_params.hpp

@@ -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 westonrobot
+{
+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 westonrobot
+
+
+#endif /* SCOUT_SIM_PARAMS_HPP */

scout_webots_sim/include/scout_webots_sim/scout_webots_interface.hpp

@@ -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 westonrobot
+{
+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 westonrobot
+
+#endif /* SCOUT_WEBOTS_INTERFACE_HPP */

scout_webots_sim/src/scout_webots_interface.cpp

@@ -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 westonrobot
+{
+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 westonrobot

scout_webots_sim/src/scout_webots_node.cpp

@@ -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 westonrobot;
+
+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;
+}