├── SFM.jpg ├── media └── logos_repo_github.jpg ├── sfm_plugin.xml ├── cfg ├── local_planner.yaml └── SFMLocalController.cfg ├── LICENSE ├── package.xml ├── CMakeLists.txt ├── include └── sfm_local_controller │ ├── sfm_controller_ros.h │ ├── sensor_interface.h │ ├── sfm_controller.h │ └── collision_checker.h ├── README.md └── src ├── sfm_controller_ros.cpp ├── collision_checker.cpp ├── sfm_controller.cpp └── sensor_interface.cpp /SFM.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/sfm_local_controller/HEAD/SFM.jpg -------------------------------------------------------------------------------- /media/logos_repo_github.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotics-upo/sfm_local_controller/HEAD/media/logos_repo_github.jpg -------------------------------------------------------------------------------- /sfm_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a simple local controller to follow a pre-defined path. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /cfg/local_planner.yaml: -------------------------------------------------------------------------------- 1 | base_local_planner: sfm_local_controller/SFMControllerROS 2 | 3 | 4 | SFMControllerROS: 5 | 6 | robot_frame: base_link 7 | planner_frame: odom 8 | 9 | max_lin_vel: 0.8 10 | min_lin_vel: 0.12 11 | max_rot_vel: !degrees 60.0 12 | min_rot_vel: !degrees 30.0 13 | min_rot_in_place: !degrees 40.0 14 | max_lin_acc: 0.15 15 | max_rot_acc: !degrees 30.0 16 | a: 5.0 17 | 18 | goal_tolerance: 0.2 19 | yaw_tolerance: 0.3 20 | local_goal_dist: 1.1 21 | robot_radius: 0.35 22 | people_radius: 0.35 23 | 24 | #sensor interface params 25 | laser_topic: scan_raw 26 | #people_topic: 27 | dyn_obs_topic: dynamic_obstacles 28 | odom_topic: mobile_base_controller/odom 29 | max_obstacle_dist: 3.0 30 | naive_goal_time: 1.0 31 | people_velocity: 1.9 32 | 33 | # sfm params 34 | sfm_goal_weight: 1.0 35 | sfm_obstacle_weight: 15.0 36 | sfm_people_weight: 8.0 37 | 38 | #collision checker 39 | use_laser: true 40 | sensor_uncertainty: 0.05 41 | use_range: false 42 | -------------------------------------------------------------------------------- /cfg/SFMLocalController.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE = 'sfm_local_controller' 4 | 5 | from math import pi 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | 12 | gen.add("max_lin_acc", double_t, 0, "The translational acceleration limit of the robot", 1.0, 0, 20.0) 13 | gen.add("max_rot_acc", double_t, 0, "The rotational acceleration limit of the robot", 1.0, 0, 20.0) 14 | gen.add("max_lin_vel", double_t, 0, "The maximum translation velocity for the robot in m/s", 0.6, 0, 10.0) 15 | gen.add("min_lin_vel", double_t, 0, "The minimum translation velocity for the robot in m/s", 0.1, 0, 10.0) 16 | gen.add("max_rot_vel", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s", 0.8, 0, 20.0) 17 | gen.add("min_rot_vel", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", 0.3, 0, 20.0) 18 | #gen.add("min_in_place_rot_vel", double_t, 0, "The absolute value of the minimum in-place rotational velocity the controller will explore", 0.5, 0, 20.0) 19 | 20 | # sfm weights 21 | gen.add("sfm_goal_weight", double_t, 0, "Weight for the attraction force to the goal", 5.0, 0, 100.0) 22 | gen.add("sfm_obstacle_weight", double_t, 0, "Weight for the repulsion force to obstacles", 15.0, 0, 100.0) 23 | gen.add("sfm_people_weight", double_t, 0, "Weight for the repulsion force to people", 12.0, 0, 100.0) 24 | 25 | 26 | exit(gen.generate(PACKAGE, "sfm_local_controller", "SFMLocalController")) 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Service Robotics Lab 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sfm_local_controller 4 | 0.0.0 5 | The sfm_local_controller package 6 | 7 | 8 | Noé Pérez 9 | 10 | 11 | BSD 12 | 13 | catkin 14 | geometry_msgs 15 | lightsfm 16 | nav_core 17 | nav_msgs 18 | pluginlib 19 | roscpp 20 | sensor_msgs 21 | tf2 22 | tf2_geometry_msgs 23 | tf2_ros 24 | visualization_msgs 25 | dynamic_reconfigure 26 | dynamic_obstacle_detector 27 | geometry_msgs 28 | lightsfm 29 | nav_core 30 | nav_msgs 31 | pluginlib 32 | roscpp 33 | sensor_msgs 34 | tf2 35 | tf2_geometry_msgs 36 | tf2_ros 37 | visualization_msgs 38 | geometry_msgs 39 | lightsfm 40 | nav_core 41 | nav_msgs 42 | pluginlib 43 | roscpp 44 | sensor_msgs 45 | tf2 46 | tf2_geometry_msgs 47 | tf2_ros 48 | visualization_msgs 49 | dynamic_reconfigure 50 | dynamic_obstacle_detector 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sfm_local_controller) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | geometry_msgs 9 | dynamic_reconfigure 10 | nav_core 11 | nav_msgs 12 | pluginlib 13 | roscpp 14 | sensor_msgs 15 | tf2 16 | tf2_geometry_msgs 17 | tf2_ros 18 | visualization_msgs 19 | dynamic_obstacle_detector 20 | ) 21 | 22 | 23 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 24 | generate_dynamic_reconfigure_options( 25 | cfg/SFMLocalController.cfg 26 | ) 27 | 28 | ################################### 29 | ## catkin specific configuration ## 30 | ################################### 31 | ## The catkin_package macro generates cmake config files for your package 32 | ## Declare things to be passed to dependent projects 33 | ## INCLUDE_DIRS: uncomment this if your package contains header files 34 | ## LIBRARIES: libraries you create in this project that dependent projects also need 35 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 36 | ## DEPENDS: system dependencies of this project that dependent projects also need 37 | catkin_package( 38 | INCLUDE_DIRS include 39 | LIBRARIES 40 | sfm_local_controller 41 | sfm_controller_ros 42 | CATKIN_DEPENDS geometry_msgs lightsfm nav_core nav_msgs pluginlib roscpp sensor_msgs tf2 tf2_geometry_msgs tf2_ros visualization_msgs 43 | # DEPENDS system_lib 44 | ) 45 | 46 | ########### 47 | ## Build ## 48 | ########### 49 | 50 | ## Specify additional locations of header files 51 | ## Your package locations should be listed before other locations 52 | include_directories( 53 | include 54 | /usr/local/include #to find lightsfm 55 | ${catkin_INCLUDE_DIRS} 56 | ) 57 | 58 | ## Declare a C++ library 59 | add_library(${PROJECT_NAME} 60 | src/sensor_interface.cpp 61 | src/collision_checker.cpp 62 | src/sfm_controller.cpp 63 | ) 64 | 65 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 66 | target_link_libraries(${PROJECT_NAME} 67 | ${catkin_LIBRARIES} 68 | ) 69 | 70 | ## Declare a C++ library 71 | add_library(sfm_controller_ros 72 | src/sfm_controller_ros.cpp 73 | ) 74 | 75 | add_dependencies(sfm_controller_ros ${sfm_controller_ros_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 76 | target_link_libraries(sfm_controller_ros 77 | ${PROJECT_NAME} 78 | ${catkin_LIBRARIES} 79 | ) 80 | 81 | 82 | install(TARGETS 83 | ${PROJECT_NAME} 84 | sfm_controller_ros 85 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | #RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 88 | ) 89 | 90 | install(FILES sfm_plugin.xml 91 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 92 | ) 93 | 94 | install(DIRECTORY include/${PROJECT_NAME}/ 95 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 96 | PATTERN ".svn" EXCLUDE 97 | ) 98 | 99 | -------------------------------------------------------------------------------- /include/sfm_local_controller/sfm_controller_ros.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Implementation of the BaseLocalPlanner plugin 3 | * in order to use the sfm_local_controller in the 4 | * ROS move_base architecture. 5 | * 6 | * Author: Noé Pérez-Higueras 7 | * Service Robotics Lab, Pablo de Olavide University 2021 8 | * 9 | * Software License Agreement (BSD License) 10 | * 11 | */ 12 | 13 | #ifndef SFM_CONTROLLER_ROS_H_ 14 | #define SFM_CONTROLLER_ROS_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | // Dynamic reconfigure 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | namespace Upo { 38 | namespace Navigation { 39 | namespace sfm_controller { 40 | /** 41 | * @class SFMControllerROS 42 | * @brief A ROS wrapper for the sfm local controller that queries the param 43 | * server to construct a controller 44 | */ 45 | class SFMControllerROS : public nav_core::BaseLocalPlanner { 46 | public: 47 | /** 48 | * @brief Default constructor for the ros wrapper 49 | */ 50 | SFMControllerROS(); 51 | /** 52 | * @brief Destructor for the wrapper 53 | */ 54 | ~SFMControllerROS(); 55 | /** 56 | * @brief Constructs the ros wrapper 57 | * @param name The name to give this instance of the trajectory planner 58 | * @param tf A pointer to a transform listener 59 | * @param costmap The cost map to use for assigning costs to trajectories. NOT 60 | * USED 61 | */ 62 | SFMControllerROS(std::string name, tf2_ros::Buffer *tf, 63 | costmap_2d::Costmap2DROS *costmap_ros); 64 | /** 65 | * @brief Constructs the ros wrapper 66 | * @param name The name to give this instance of the trajectory planner 67 | * @param tf A pointer to a transform listener 68 | * @param costmap The cost map to use for assigning costs to trajectories. NOT 69 | * USED 70 | */ 71 | void initialize(std::string name, tf2_ros::Buffer *tf, 72 | costmap_2d::Costmap2DROS *costmap_ros); 73 | /** 74 | * @brief Given the current position, orientation, and velocity of the robot, 75 | * compute velocity commands to send to the base 76 | * @param cmd_vel Will be filled with the velocity command to be passed to the 77 | * robot base 78 | * @return True if a valid trajectory was found, false otherwise 79 | */ 80 | bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel); 81 | /** 82 | * @brief Set the plan that the controller is following 83 | * @param orig_global_plan The plan to pass to the controller 84 | * @return True if the plan was updated successfully, false otherwise 85 | */ 86 | bool setPlan(const std::vector &orig_global_plan); 87 | 88 | /** 89 | * @brief Check if the goal pose has been achieved 90 | * @return True if achieved, false otherwise 91 | */ 92 | bool isGoalReached(); 93 | 94 | // void scanCb(const sensor_msgs::LaserScan::ConstPtr &laser); 95 | 96 | private: 97 | /** 98 | * @brief Callback to update the local planner's parameters based on dynamic 99 | * reconfigure 100 | */ 101 | void reconfigureCB(sfm_local_controller::SFMLocalControllerConfig &config, 102 | uint32_t level); 103 | 104 | std::vector 105 | transformPlan(const std::vector &plan, 106 | std::string frame); 107 | 108 | // ROS parameters 109 | tf2_ros::Buffer *tf_buffer_; 110 | std::string planner_frame_; 111 | std::string robot_frame_; 112 | ros::Publisher g_plan_pub_, l_plan_pub_; 113 | 114 | // ros::Subscriber odom_sub_; 115 | // ros::Subscriber laser_sub_, people_sub_, dyn_obs_sub_, sonar_sub_; 116 | 117 | std::vector planner_path_; 118 | bool goal_reached_; 119 | 120 | // dynamic reconfigure 121 | dynamic_reconfigure::Server 122 | *dsrv_; 123 | sfm_local_controller::SFMLocalControllerConfig default_config_; 124 | 125 | // Controller 126 | SFMController *sfm_controller_; 127 | SFMSensorInterface *sensor_iface_; 128 | CollisionChecker *collision_checker_; 129 | bool initialized_; 130 | }; 131 | 132 | } // namespace sfm_controller 133 | } // namespace Navigation 134 | } // namespace Upo 135 | 136 | #endif 137 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # sfm_local_controller 2 | A robot local controller based on the Social Force Model (SFM) (https://github.com/robotics-upo/lightsfm). 3 | A set of attractive and repulsive forces determine the behavior of the planner. 4 | 5 | This local planner has been programmed as a BaseLocalPlanner plugin. So it can be used in the ROS move_base architecture. 6 | Although It receives a local costmap from move_base, the costmap is not used and a dedicated collision detector is employed instead. This detector uses laserscan input to check the possible collisions. 7 | 8 | ![](https://github.com/robotics-upo/sfm_local_controller/blob/master/SFM.jpg) 9 | 10 | 11 | ## Acknowledgment 12 | 13 | This work has been financed by the European Regional Development Fund (FEDER) and by the Ministry of Economy, Knowledge, Business and University, of the Government of Andalucía , within the framework of the FEDER Andalucía 2014-2020 operational program. Specific objective 1.2.3. "Promotion and generation of frontier knowledge and knowledge oriented to the challenges of society, development of emerging technologies" within the framework of the reference research project UPO-1264631. 14 | FEDER co-financing percentage 80% 15 |

16 | 17 | ## Parameters 18 | 19 | * **Robot Configuration Parameters** 20 | - *max_lin_acc*. Maximum acceleration in translation (m/s^2). 21 | - *max_rot_acc*. Maximum acceleration in rotation (rad/s^2). 22 | - *max_lin_vel*. Maximum linear velocity (m/s). 23 | - *min_lin_vel*. Minimum linear velocity (m/s). 24 | - *max_rot_vel*. Maximum angular velocity (rad/s). 25 | - *min_rot_vel*. Minimum angular velocity (rad/s). 26 | - *min_rot_in_place*. Angular velocity of rotations in place (rad/s). 27 | - *a*. A factor that determines the angular velocity when the robot is turning. (Def: 10.0). 28 | - *planner_frame*. The frame in which the controller works. (Def: "odom"). 29 | - *robot_frame*. The robot base frame. (Def: "base_link"). 30 | - *robot_radius*. Radius (m) of the approximated circumference of the robot footprint. 31 | - *people_radius*. Radius (m) of the approximated circumference that circumscribe a human footprint. (Def: 0.35). 32 | 33 | * **Goal Tolerance Parameters** 34 | - *goal_tolerance*. Tolerance in euclidean distance (m) to consider that the goal has been reached. 35 | - *yaw_tolerance*. Tolerance in angular distance (rad) to consider that the goal has been reached. 36 | - *local_goal_dist*. Distance (m) from the robot to look for a local goal in the global path (Def: 1.0). 37 | 38 | * **Sensor Interface Parameters** 39 | The sensor interface is in charge of taking the sensory input and update the information of the surrounding social agents in the scene. 40 | - *laser_topic*. Topic in which the laser range finder is being published [sensor_msgs/LaserScan]. 41 | - *people_topic*. Topic in which the people detected are being published [people_msgs/People]. 42 | - *dyn_obs_topic*. Topic in which the dynamic obstacles detected are being published [dynamic_obstacle_detector/DynamicObstacles]. 43 | - *odom_topic*. Odometry topic [nav_msgs/Odometry]. 44 | - *max_obstacle_dist*. Maximum distance (m) in which the obstacles are considered for the social force model. 45 | - *naive_goal_time*. Lookahead time (secs) to predict an approximate goal for the pedestrians. 46 | - *people_velocity*. Average velocity of the pedestrians (m/s). 47 | 48 | * **Social Force Model Parameters** 49 | - *sfm_goal_weight*. Weight of the attraction force to the goal. 50 | - *sfm_obstacle_weight*. Weight of the respulsive force of the obstacles. 51 | - *sfm_people_weight*. Weight of the respulsive force of the pedestrians. 52 | 53 | * **Collision Checker Parameters** 54 | - *use_laser*. Boolean to indicate whether to use a laser scan for input. The scan topic indicated in the previous parameter *laser_topic* will be used. (Def: True). 55 | - *use_range*. Boolean to indicate whether to use sonar ranges for collision detection. If True, the topic names of the different sonars must be indicated by the parameters *range_topic_X* in which the *X* must be replaced by a number. eg: *range_topic_0*, range_topic_1*, etc. (Def: False). 56 | - *sensor_uncertainty*. Uncertainty or accurary of the sensory input (m). (Def: 0.05). 57 | 58 | ## Publications 59 | 60 | Besides the publications of the commands and paths according to the move_base structure. The planner publishes some RViz markers: 61 | 62 | - */sfm/markers/goal* [visualization_msgs/Marker]. Arrow with the local goal of the controller. 63 | 64 | - */sfm/markers/robot_forces* [visualization_msgs/MarkerArray]. Vectors with the different forces of the SFM. 65 | 66 | ## Dependencies 67 | 68 | - dynamic_obstacle_detector (https://github.com/robotics-upo/dynamic_obstacle_detector). 69 | 70 | - lightsfm (https://github.com/robotics-upo/lightsfm) 71 | 72 | 73 | -------------------------------------------------------------------------------- /include/sfm_local_controller/sensor_interface.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Class to adapt the sensory input and detections to be used by the 3 | * sfm_local_controller 4 | * 5 | * Author: Noé Pérez-Higueras 6 | * Service Robotics Lab, Pablo de Olavide University 2021 7 | * 8 | * Software License Agreement (BSD License) 9 | * 10 | */ 11 | 12 | #ifndef SFMCONTROLLER_SENSOR_H_ 13 | #define SFMCONTROLLER_SENSOR_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | // sensor input for obstacles 25 | #include 26 | // Detection input for social navigation 27 | #include 28 | #include 29 | // Social Force Model 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | namespace Upo { 38 | namespace Navigation { 39 | namespace sfm_controller { 40 | 41 | class SFMSensorInterface { 42 | public: 43 | /** 44 | * @brief Default constructor 45 | * @param n pointer to a ros node handle to publish to topics 46 | * @param tfBuffer Pointer to tf2 buffer 47 | * @param robot_max_lin_speed the maximum robot linear speed [m/s] 48 | * @param robot_radius the radius of the robot circunference [m] 49 | * @param person_radius the approximated radius of the people body 50 | * @param robot_frame the coordinate frame of the robot base 51 | * @param odom_frame the coordinate frame of the robot odometry 52 | **/ 53 | SFMSensorInterface(ros::NodeHandle *n, tf2_ros::Buffer *tfBuffer, 54 | float robot_max_lin_speed, float robot_radius, 55 | float person_radius, std::string robot_frame, 56 | std::string odom_frame); 57 | 58 | /** 59 | * @brief Destructor class 60 | */ 61 | ~SFMSensorInterface(); 62 | 63 | /** 64 | * @brief Callback to process the laser scan sensory input. 65 | * @param laser laserScan message to be processed 66 | */ 67 | void laserCb(const sensor_msgs::LaserScan::ConstPtr &laser); 68 | 69 | /** 70 | * @brief Callback to process the people detected in the robot vecinity. 71 | * @param people message with the people to be processed 72 | */ 73 | void peopleCb(const people_msgs::People::ConstPtr &people); 74 | 75 | /** 76 | * @brief Callback to process the moving obstacles detected in the robot 77 | * vecinity. 78 | * @param obs messages with the obstacles to be processed. 79 | */ 80 | void dynamicObsCb( 81 | const dynamic_obstacle_detector::DynamicObstacles::ConstPtr &obs); 82 | 83 | /** 84 | * @brief Callback to process the odometry messages with the robot movement. 85 | * @param odom messages with the obstacles to be processed. 86 | */ 87 | void odomCb(const nav_msgs::Odometry::ConstPtr &odom); 88 | 89 | /** 90 | * @brief Tranform a coordinate vector from one frame to another 91 | * @param vector coordinate vector in the origin frame 92 | * @param from string with the name of the origin frame 93 | * @param to string with the name of the target frame 94 | * @return coordinate vector in the target frame 95 | */ 96 | geometry_msgs::Vector3 transformVector(geometry_msgs::Vector3 &vector, 97 | std::string from, std::string to); 98 | 99 | /** 100 | * @brief returns the vector of sfm agents 101 | * @return agents vector 102 | */ 103 | std::vector getAgents(); 104 | 105 | void setMaxLinVel(float max_lin_vel) { 106 | agents_mutex_.lock(); 107 | agents_[0].desiredVelocity = max_lin_vel; 108 | agents_mutex_.unlock(); 109 | } 110 | 111 | private: 112 | /** 113 | * @brief publish the transformed points in RViz 114 | * @param points vector with the coordinates of the points 115 | * @return none 116 | */ 117 | void publish_obstacle_points(const std::vector &points); 118 | 119 | // void updateAgents(); 120 | 121 | ros::NodeHandle *nh_; // Pointer to the node node handle 122 | ros::NodeHandle n_; 123 | tf2_ros::Buffer *tf_buffer_; // Pointer to the tfBuffer created in the node 124 | 125 | ros::Subscriber odom_sub_; 126 | std::mutex odom_mutex_; 127 | ros::Subscriber laser_sub_, people_sub_, dyn_obs_sub_, sonar_sub_; 128 | ros::Publisher points_pub_; 129 | 130 | std::vector agents_; // 0: robot, 1..: Others 131 | // sfm::Agent robot_agent_; 132 | std::mutex agents_mutex_; 133 | std::vector obstacles_; 134 | 135 | bool laser_received_; 136 | ros::Time last_laser_; 137 | 138 | people_msgs::People people_; 139 | std::mutex people_mutex_; 140 | dynamic_obstacle_detector::DynamicObstacles dyn_obs_; 141 | std::mutex obs_mutex_; 142 | 143 | float person_radius_; 144 | float naive_goal_time_; 145 | float people_velocity_; 146 | 147 | float max_obstacle_dist_; 148 | 149 | std::string robot_frame_; 150 | std::string odom_frame_; 151 | 152 | // bool use_static_map_; 153 | // sfm::RosMap *static_map_; 154 | }; 155 | 156 | } // namespace sfm_controller 157 | } // namespace Navigation 158 | } // namespace Upo 159 | 160 | #endif -------------------------------------------------------------------------------- /include/sfm_local_controller/sfm_controller.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Local robot controller based on the Social Force Model 3 | * 4 | * Author: Noé Pérez-Higueras 5 | * Service Robotics Lab, Pablo de Olavide University 2021 6 | * 7 | * Software License Agreement (BSD License) 8 | * 9 | */ 10 | 11 | #ifndef SFM_CONTROLLER_H_ 12 | #define SFM_CONTROLLER_H_ 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include // for copy() and assign() 23 | #include // for back_inserter 24 | 25 | namespace Upo { 26 | namespace Navigation { 27 | namespace sfm_controller { 28 | class SFMController { 29 | public: 30 | /** 31 | * @brief Default constructor 32 | */ 33 | SFMController(); 34 | /** 35 | * @brief Constructor with parameters 36 | */ 37 | SFMController(ros::NodeHandle *n, tf2_ros::Buffer *tfBuffer, 38 | SFMSensorInterface *sensor_iface, CollisionChecker *coll_check, 39 | float max_lin_vel, float min_lin_vel, float max_vel_theta, 40 | float min_vel_theta, float min_rot_in_place, float max_lin_acc, 41 | float max_theta_acc, float local_goal_dist, 42 | float sfm_goal_weight, float sfm_obstacle_weight, 43 | float sfm_people_weight, float robot_radius, 44 | float person_radius, float goal_tolerance, float yaw_tolerance, 45 | std::string robot_frame, std::string controller_frame, float a); 46 | /** 47 | * @brief Default destructor 48 | */ 49 | ~SFMController(); 50 | 51 | /** 52 | * @brief Callback to update the local planner's parameters based on dynamic 53 | * reconfigure 54 | */ 55 | void reconfigure(sfm_local_controller::SFMLocalControllerConfig &cfg); 56 | 57 | /** 58 | * @brief method to update the scenario situation 59 | * @param path global path to be followed by the local controller 60 | * @return True when the goal has been reached, False otherwise 61 | */ 62 | bool update(std::vector path); 63 | /** 64 | * @brief method to compute the velocity command of the robot 65 | * @param cmd_vel velocity message to be filled 66 | * @return True if a command vel was found 67 | */ 68 | bool computeAction(geometry_msgs::Twist &cmd_vel); 69 | 70 | /** 71 | * @brief check if the current scenario leads to a possible collision 72 | * @return True if a possible collision was detected, False otherwise 73 | */ 74 | bool fastCollisioncheck(); 75 | 76 | private: 77 | /** 78 | * @brief inline method of normalize an angle 79 | * @param val the angle in rads to be normalized 80 | * @param min the min angle allowed 81 | * @param max the max angle allowed 82 | * @return the normalized angle value 83 | */ 84 | float inline normalizeAngle(float val, float min, float max) { 85 | float norm = 0.0; 86 | if (val >= min) 87 | norm = min + fmod((val - min), (max - min)); 88 | else 89 | norm = max - fmod((min - val), (max - min)); 90 | 91 | return norm; 92 | } 93 | /** 94 | * @brief Compute an angular vel to be sent to the robot 95 | * @param max maximum angular vel allowed 96 | * @param exp_const parameter of the function 97 | * @param var the angle between the robot heading and the goal heading 98 | * @return the angular vel 99 | */ 100 | float inline getVel(float max, float exp_const, float var) { 101 | return max * (1 - exp(-exp_const * fabs(var))) * var / fabs(var); 102 | }; 103 | float a_; 104 | 105 | /** 106 | * @brief Publish an arrow marker in Rviz representing a force 107 | * @param index id of the marker 108 | * @param color RGB color of the marker 109 | * @param force force to be represented 110 | * @param markers markerArray in which the arrow will be added 111 | * @return none 112 | */ 113 | void publishForceMarker(unsigned index, const std_msgs::ColorRGBA &color, 114 | const utils::Vector2d &force, 115 | visualization_msgs::MarkerArray &markers); 116 | /** 117 | * @brief Publish the set of SFM forces in RViz 118 | * @return none 119 | */ 120 | void publishForces(); 121 | /** 122 | * @brief fill a ColorRGBA message 123 | * @param r value of the red componet [0-1] 124 | * @param g value of the green component [0-1] 125 | * @param b value of the blue component [0-1] 126 | * @param a transparency of the color [0-1] 127 | * @return a ROS ColorRGBA message 128 | */ 129 | std_msgs::ColorRGBA getColor(double r, double g, double b, double a); 130 | 131 | /** 132 | * @brief Publish in RViz the local goal followed by the controller 133 | * @param g goal position 134 | * @return none 135 | */ 136 | void publishSFMGoal(const geometry_msgs::PoseStamped &g); 137 | 138 | SFMSensorInterface *sensor_iface_; 139 | CollisionChecker *collision_checker_; 140 | 141 | std::mutex configuration_mutex_; 142 | 143 | std::string planner_frame_; 144 | std::string robot_frame_; 145 | ros::Publisher g_plan_pub_, l_plan_pub_, robot_markers_pub_, 146 | sfm_goal_pub_; //, vel_pub_; 147 | 148 | sfm::Agent robot_; 149 | std::vector agents_; 150 | geometry_msgs::Pose goal_; 151 | std::vector planner_path_; 152 | bool goal_reached_; 153 | bool rotate_; 154 | 155 | ros::Time last_command_time_; 156 | 157 | // Robot configuration parameters 158 | float max_lin_vel_, min_lin_vel_; 159 | float max_vel_theta_, min_vel_theta_; 160 | float min_rot_in_place_; 161 | float max_lin_acc_; 162 | float max_theta_acc_; 163 | float robot_radius_; 164 | float person_radius_; 165 | float goal_tolerance_; 166 | float yaw_tolerance_; 167 | float local_goal_dist_; 168 | 169 | // SFM weights 170 | float sfm_goal_weight_; 171 | float sfm_obstacle_weight_; 172 | float sfm_people_weight_; 173 | }; // namespace sfm_controller 174 | } // namespace sfm_controller 175 | } // namespace Navigation 176 | } // namespace Upo 177 | 178 | #endif -------------------------------------------------------------------------------- /include/sfm_local_controller/collision_checker.h: -------------------------------------------------------------------------------- 1 | #ifndef COLLISION_CHECKER_H_ 2 | #define COLLISION_CHECKER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace Upo { 24 | namespace Navigation { 25 | namespace sfm_controller { 26 | 27 | class CollisionChecker { 28 | 29 | public: 30 | struct range { 31 | std::string id; 32 | float range; 33 | float polar_angle; 34 | float polar_dist; 35 | float fov; 36 | float min_dist; 37 | float max_dist; 38 | }; 39 | 40 | CollisionChecker(ros::NodeHandle *n, tf2_ros::Buffer *tf, double max_lv, 41 | double max_av, double lin_acc, double ang_acc, 42 | double r_radius, std::string base_frame, 43 | std::string planner_frame); 44 | 45 | ~CollisionChecker(); 46 | 47 | void setup(); 48 | 49 | /** 50 | * @brief Generate and check a single trajectory 51 | * @param cvx The current x velocity of the robot 52 | * @param cvy The current y velocity of the robot 53 | * @param cvth The current angular velocity of the robot 54 | * @param tvx The x velocity used to seed the trajectory 55 | * @param tvy The y velocity used to seed the trajectory 56 | * @param tvth The theta velocity used to seed the trajectory 57 | * @param dt The time for project the movement 58 | * @return True if the trajectory is legal, false otherwise 59 | */ 60 | bool checkCommand(double cvx, double cvy, double cvth, double tvx, double tvy, 61 | double tvth, double dt); 62 | 63 | // void saturateVelocities(geometry_msgs::Twist *twist); 64 | 65 | void setVelParams(double max_lin_vel, double max_vel_theta, 66 | double max_lin_acc, double max_theta_acc) { 67 | params_mutex_.lock(); 68 | max_lin_vel_ = max_lin_vel; 69 | max_ang_vel_ = max_vel_theta; 70 | max_lin_acc_ = max_lin_acc; 71 | max_ang_acc_ = max_theta_acc; 72 | params_mutex_.unlock(); 73 | } 74 | 75 | void laserCallback(const sensor_msgs::LaserScan::ConstPtr &msg); 76 | 77 | std::vector 78 | laser_polar2euclidean(const sensor_msgs::LaserScan *scan); 79 | 80 | void transformPoints(std::vector &points, 81 | std::string input_frame); 82 | 83 | void rangeCallback(const sensor_msgs::Range::ConstPtr &msg); 84 | void initiateRanges(); 85 | 86 | bool inRangeCollision(float x, float y); 87 | bool inLaserCollision(float x, float y, 88 | const std::vector &scanpoints); 89 | bool inCollision(float x, float y, 90 | const std::vector &scanpoints); 91 | 92 | private: 93 | float inline normalizeAngle(float val, float min, float max) { 94 | float norm = 0.0; 95 | if (val >= min) 96 | norm = min + fmod((val - min), (max - min)); 97 | else 98 | norm = max - fmod((min - val), (max - min)); 99 | 100 | return norm; 101 | } 102 | 103 | /** 104 | * @brief Compute x position based on velocity 105 | * @param xi The current x position 106 | * @param vx The current x velocity 107 | * @param vy The current y velocity 108 | * @param theta The current orientation 109 | * @param dt The timestep to take 110 | * @return The new x position 111 | */ 112 | inline double computeNewXPosition(double xi, double vx, double vy, 113 | double theta, double dt) { 114 | return xi + (vx * cos(theta) + vy * cos(M_PI_2 + theta)) * dt; 115 | } 116 | 117 | /** 118 | * @brief Compute y position based on velocity 119 | * @param yi The current y position 120 | * @param vx The current x velocity 121 | * @param vy The current y velocity 122 | * @param theta The current orientation 123 | * @param dt The timestep to take 124 | * @return The new y position 125 | */ 126 | inline double computeNewYPosition(double yi, double vx, double vy, 127 | double theta, double dt) { 128 | return yi + (vx * sin(theta) + vy * sin(M_PI_2 + theta)) * dt; 129 | } 130 | 131 | /** 132 | * @brief Compute orientation based on velocity 133 | * @param thetai The current orientation 134 | * @param vth The current theta velocity 135 | * @param dt The timestep to take 136 | * @return The new orientation 137 | */ 138 | inline double computeNewThetaPosition(double thetai, double vth, double dt) { 139 | return thetai + vth * dt; 140 | } 141 | 142 | // compute velocity based on acceleration 143 | /** 144 | * @brief Compute velocity based on acceleration 145 | * @param vg The desired velocity, what we're accelerating up to 146 | * @param vi The current velocity 147 | * @param a_max An acceleration limit 148 | * @param dt The timestep to take 149 | * @return The new velocity 150 | */ 151 | inline double computeNewVelocity(double vg, double vi, double a_max, 152 | double dt) { 153 | if ((vg - vi) >= 0) { 154 | return std::min(vg, vi + a_max * dt); 155 | } 156 | return std::max(vg, vi - a_max * dt); 157 | } 158 | 159 | // tf::TransformListener* tf_; 160 | tf2_ros::Buffer *tf_; 161 | 162 | ros::NodeHandle *nh_; // Pointer to the node node handle 163 | ros::NodeHandle n_; 164 | 165 | std::string odom_topic_; 166 | std::string robot_frame_; 167 | std::string planner_frame_; 168 | 169 | double max_lin_vel_; 170 | double max_ang_vel_; 171 | double max_lin_acc_; 172 | double max_ang_acc_; 173 | // double sim_time_; 174 | double robot_radius_; 175 | double robot_radius_aug_; 176 | // double local_radius_; 177 | // double granularity_; 178 | std::mutex params_mutex_; 179 | 180 | // float max_lv_var_; 181 | // float max_av_var_; 182 | 183 | // tf::Stamped robot_vel_; 184 | 185 | // in case a laser range finder is also used 186 | bool use_laser_; 187 | ros::Subscriber laser_sub_; 188 | // sensor_msgs::LaserScan laser_scan_; 189 | std::vector scanpoints_; 190 | std::mutex laser_mutex_; 191 | 192 | // Sonar ranges employed 193 | bool use_range_; 194 | int num_ranges_; 195 | std::vector range_topics_; 196 | std::vector range_frames_; 197 | std::vector range_subscribers_; 198 | // ros::Subscriber range_sub_; 199 | std::vector ranges_; 200 | std::mutex range_mutex_; 201 | std::vector ranges_initiated_; 202 | bool ranges_ready_; 203 | }; 204 | 205 | } // namespace sfm_controller 206 | } // namespace Navigation 207 | } // namespace Upo 208 | #endif 209 | -------------------------------------------------------------------------------- /src/sfm_controller_ros.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Implementation of the BaseLocalPlanner plugin 3 | * in order to use the sfm_local_controller in the 4 | * ROS move_base architecture. 5 | * 6 | * Author: Noé Pérez-Higueras 7 | * Service Robotics Lab, Pablo de Olavide University 2021 8 | * 9 | * Software License Agreement (BSD License) 10 | * 11 | */ 12 | 13 | #include 14 | 15 | //#include 16 | 17 | //#include 18 | 19 | #include 20 | #include 21 | 22 | // register this planner as a BaseLocalPlanner plugin 23 | PLUGINLIB_EXPORT_CLASS(Upo::Navigation::sfm_controller::SFMControllerROS, 24 | nav_core::BaseLocalPlanner) 25 | 26 | namespace Upo { 27 | namespace Navigation { 28 | namespace sfm_controller { 29 | 30 | void SFMControllerROS::reconfigureCB( 31 | sfm_local_controller::SFMLocalControllerConfig &config, uint32_t level) { 32 | 33 | if (initialized_) 34 | sfm_controller_->reconfigure(config); 35 | } 36 | 37 | SFMControllerROS::SFMControllerROS() 38 | : initialized_(false), sfm_controller_(nullptr) {} 39 | 40 | SFMControllerROS::SFMControllerROS(std::string name, tf2_ros::Buffer *tf, 41 | costmap_2d::Costmap2DROS *costmap_ros) 42 | : initialized_(false), sfm_controller_(nullptr) { 43 | // initialize the planner 44 | initialize(name, tf, costmap_ros); 45 | } 46 | 47 | void SFMControllerROS::initialize(std::string name, tf2_ros::Buffer *tf, 48 | costmap_2d::Costmap2DROS *costmap_ros) { 49 | if (!initialized_) { 50 | ros::NodeHandle private_nh("~/" + name); 51 | g_plan_pub_ = private_nh.advertise("global_plan", 1); 52 | l_plan_pub_ = private_nh.advertise("local_plan", 1); 53 | 54 | tf_buffer_ = tf; 55 | 56 | // Frames 57 | private_nh.param("robot_frame", robot_frame_, std::string("base_link")); 58 | private_nh.param("planner_frame", planner_frame_, std::string("odom")); 59 | 60 | // Robot Configuration Parameters 61 | float max_lin_vel_, min_lin_vel_; 62 | float max_vel_theta_, min_vel_theta_; 63 | float min_rot_in_place_; 64 | float max_lin_acc_; 65 | float max_theta_acc_; 66 | private_nh.param("max_lin_vel", max_lin_vel_, float(0.6)); 67 | private_nh.param("min_lin_vel", min_lin_vel_, float(0.1)); 68 | private_nh.param("max_rot_vel", max_vel_theta_, float(0.5)); 69 | private_nh.param("min_rot_vel", min_vel_theta_, float(0.1)); 70 | private_nh.param("min_rot_in_place", min_rot_in_place_, float(0.35)); 71 | private_nh.param("max_lin_acc", max_lin_acc_, float(1.0)); 72 | private_nh.param("max_rot_acc", max_theta_acc_, float(1.0)); 73 | 74 | // Dimensions 75 | float robot_radius; 76 | float people_radius; 77 | private_nh.param("robot_radius", robot_radius, float(0.35)); 78 | private_nh.param("people_radius", people_radius, float(0.35)); 79 | float robot_goal_dist; 80 | private_nh.param("local_goal_dist", robot_goal_dist, float(1.0)); 81 | 82 | float a; 83 | private_nh.param("a", a, float(3.0)); 84 | 85 | // Goal tolerance parameters 86 | float goal_tolerance, yaw_tolerance; 87 | private_nh.param("goal_tolerance", goal_tolerance, float(0.2)); 88 | private_nh.param("yaw_tolerance", yaw_tolerance, float(0.35)); 89 | // private_nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.10); 90 | // private_nh.param("wp_tolerance", wp_tolerance_, 0.5); 91 | 92 | // SFM weights 93 | float sfm_goal_weight_; 94 | float sfm_obstacle_weight_; 95 | float sfm_people_weight_; 96 | private_nh.param("sfm_goal_weight", sfm_goal_weight_, float(2.0)); 97 | private_nh.param("sfm_obstacle_weight", sfm_obstacle_weight_, float(20.0)); 98 | private_nh.param("sfm_people_weight", sfm_people_weight_, float(12.0)); 99 | 100 | // std::string laser_topic; 101 | // private_nh.param("laser_topic", laser_topic, std::string("scan")); 102 | // std::string people_topic; 103 | // private_nh.param("people_topic", people_topic, std::string("people")); 104 | // std::string obs_topic; 105 | // private_nh.param("dyn_obs_topic", obs_topic, std::string("obstacles")); 106 | // std::string odom_topic; 107 | // private_nh.param("odom_topic", odom_topic, std::string("odom")); 108 | 109 | sensor_iface_ = 110 | new SFMSensorInterface(&private_nh, tf, max_lin_vel_, robot_radius, 111 | people_radius, robot_frame_, planner_frame_); 112 | 113 | collision_checker_ = new CollisionChecker( 114 | &private_nh, tf, max_lin_vel_, max_vel_theta_, max_lin_acc_, 115 | max_theta_acc_, robot_radius, robot_frame_, planner_frame_); 116 | 117 | // ros::NodeHandle n; 118 | // laser_sub_ = n.subscribe( 119 | // laser_topic.c_str(), 1, &SFMSensorInterface::laserCb, sensor_iface_); 120 | 121 | // people_sub_ = n.subscribe( 122 | // people_topic.c_str(), 1, &SFMSensorInterface::peopleCb, 123 | // sensor_iface_); 124 | 125 | // dyn_obs_sub_ = n.subscribe( 126 | // obs_topic.c_str(), 1, &SFMSensorInterface::dynamicObsCb, 127 | // sensor_iface_); 128 | 129 | // odom_sub_ = n.subscribe( 130 | // odom_topic.c_str(), 1, &SFMSensorInterface::odomCb, sensor_iface_); 131 | 132 | // std::cout << std::endl 133 | // << "SFM SENSOR INTERFACE:" << std::endl 134 | // << "laser_topic: " << laser_topic << std::endl 135 | // << "people_topic: " << people_topic << std::endl 136 | // << "dyn_obs_topic: " << obs_topic << std::endl 137 | // << "odom_topic: " << odom_topic 138 | // << std::endl 139 | // //<< "max_obstacle_dist: " << max_obstacle_dist_ << std::endl 140 | // //<< "naive_goal_time: " << naive_goal_time_ << std::endl 141 | // //<< "people_velocity: " << people_velocity_ << std::endl 142 | // << std::endl; 143 | 144 | sfm_controller_ = new SFMController( 145 | &private_nh, tf, sensor_iface_, collision_checker_, max_lin_vel_, 146 | min_lin_vel_, max_vel_theta_, min_vel_theta_, min_rot_in_place_, 147 | max_lin_acc_, max_theta_acc_, robot_goal_dist, sfm_goal_weight_, 148 | sfm_obstacle_weight_, sfm_people_weight_, robot_radius, people_radius, 149 | goal_tolerance, yaw_tolerance, robot_frame_, planner_frame_, a); 150 | 151 | // BE CAREFUL, this will load the values of cfg params 152 | // overwritting the read ones from the yaml file (if planner already 153 | // initialized). 154 | dsrv_ = new dynamic_reconfigure::Server< 155 | sfm_local_controller::SFMLocalControllerConfig>(private_nh); 156 | dynamic_reconfigure::Server< 157 | sfm_local_controller::SFMLocalControllerConfig>::CallbackType cb = 158 | boost::bind(&SFMControllerROS::reconfigureCB, this, _1, _2); 159 | dsrv_->setCallback(cb); 160 | 161 | initialized_ = true; 162 | goal_reached_ = false; 163 | 164 | } else { 165 | ROS_WARN("This planner has already been initialized, doing nothing"); 166 | } 167 | } 168 | 169 | SFMControllerROS::~SFMControllerROS() { 170 | // make sure to clean things up 171 | delete dsrv_; 172 | 173 | if (sfm_controller_ != nullptr) 174 | delete sfm_controller_; 175 | } 176 | 177 | bool SFMControllerROS::isGoalReached() { return goal_reached_; } 178 | 179 | bool SFMControllerROS::setPlan( 180 | const std::vector &orig_global_plan) { 181 | if (!initialized_) { 182 | ROS_ERROR("This planner has not been initialized, please call initialize() " 183 | "before using this local planner"); 184 | return false; 185 | } 186 | 187 | planner_path_.clear(); 188 | // Transform plan to planner_frame_ if necessary 189 | if (!orig_global_plan.empty()) { 190 | // if (orig_global_plan[0].header.frame_id != planner_frame_) { 191 | // planner_path_ = transformPlan(orig_global_plan, planner_frame_); 192 | //} else { 193 | planner_path_ = orig_global_plan; 194 | // planner_path_ = transformPlan(planner_path_, planner_frame_); 195 | //} 196 | } 197 | // reset the goal flag 198 | goal_reached_ = false; 199 | 200 | return true; 201 | } 202 | 203 | std::vector SFMControllerROS::transformPlan( 204 | const std::vector &plan, std::string frame) { 205 | std::vector planner_plan; 206 | for (unsigned int i = 0; i < plan.size(); i++) { 207 | try { 208 | geometry_msgs::PoseStamped pose = tf_buffer_->transform(plan[i], frame); 209 | planner_plan.push_back(pose); 210 | } catch (tf2::TransformException &ex) { 211 | ROS_WARN("Could NOT transform plan pose to %s: %s", frame.c_str(), 212 | ex.what()); 213 | break; 214 | } 215 | } 216 | return planner_plan; 217 | } 218 | 219 | bool SFMControllerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) { 220 | if (!initialized_) { 221 | ROS_ERROR("This planner has not been initialized, please call initialize() " 222 | "before using this planner"); 223 | return false; 224 | } 225 | 226 | // if the global plan passed in is empty... we won't do anything 227 | if (planner_path_.empty()) { 228 | ROS_WARN("Planner path is empty. Not computing cmd vels!!!!"); 229 | return false; 230 | } 231 | 232 | planner_path_ = transformPlan(planner_path_, planner_frame_); 233 | goal_reached_ = sfm_controller_->update(planner_path_); 234 | 235 | geometry_msgs::Twist drive_cmds; 236 | if (goal_reached_) { 237 | ROS_INFO("GOAL REACHED!\n\n"); 238 | cmd_vel = drive_cmds; 239 | cmd_vel.linear.x = 0.0; 240 | cmd_vel.linear.y = 0.0; 241 | cmd_vel.angular.z = 0.0; 242 | return true; 243 | } 244 | // compute what trajectory to drive along 245 | bool ok = sfm_controller_->computeAction(drive_cmds); 246 | // pass along drive commands 247 | cmd_vel = drive_cmds; 248 | if (!ok) { 249 | ROS_DEBUG_NAMED("trajectory_planner_ros", 250 | "The rollout planner failed to find a valid plan. This " 251 | "means that the footprint of the robot was in collision " 252 | "for all simulated trajectories."); 253 | // publishPlan(transformed_plan, g_plan_pub_); 254 | return false; 255 | } 256 | // publish information to the visualizer 257 | // publishPlan(transformed_plan, g_plan_pub_); 258 | 259 | return true; 260 | } 261 | 262 | } // namespace sfm_controller 263 | } // namespace Navigation 264 | } // namespace Upo -------------------------------------------------------------------------------- /src/collision_checker.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | namespace Upo { 5 | namespace Navigation { 6 | namespace sfm_controller { 7 | 8 | CollisionChecker::CollisionChecker(ros::NodeHandle *n, tf2_ros::Buffer *tf, 9 | double max_lv, double max_av, double lin_acc, 10 | double ang_acc, double r_radius, 11 | std::string base_frame, 12 | std::string planner_frame) 13 | : nh_(n), n_(), tf_(tf), max_lin_vel_(max_lv), max_ang_vel_(max_av), 14 | max_lin_acc_(lin_acc), max_ang_acc_(ang_acc), robot_radius_(r_radius), 15 | robot_frame_(base_frame), planner_frame_(planner_frame) { 16 | 17 | robot_radius_aug_ = robot_radius_; 18 | 19 | use_laser_ = true; 20 | use_range_ = false; 21 | num_ranges_ = 0; 22 | ranges_ready_ = false; 23 | 24 | setup(); 25 | } 26 | 27 | CollisionChecker::~CollisionChecker() {} 28 | 29 | void CollisionChecker::setup() { 30 | 31 | double uncertainty; 32 | nh_->param("sensor_uncertainty", uncertainty, 0.05); 33 | robot_radius_aug_ = robot_radius_ + uncertainty; 34 | 35 | nh_->param("use_laser", use_laser_, true); 36 | std::string laser_topic; 37 | if (use_laser_) { 38 | nh_->param("laser_topic", laser_topic, std::string("scan")); 39 | laser_sub_ = n_.subscribe( 40 | laser_topic.c_str(), 1, &CollisionChecker::laserCallback, this); 41 | } 42 | 43 | unsigned int i = 0; 44 | nh_->param("use_range", use_range_, false); 45 | if (use_range_) { 46 | bool ok = true; 47 | while (ok) { 48 | char buf[25]; 49 | sprintf(buf, "range_topic_%u", i); 50 | std::string st = std::string(buf); 51 | 52 | if (nh_->hasParam(st.c_str())) { 53 | std::string rt; 54 | nh_->getParam(st.c_str(), rt); 55 | range_topics_.push_back(rt); 56 | // ros::Subscriber sub = nh.subscribe(rt.c_str(), 57 | // 10, &CollisionDetection::rangeCallback, this); 58 | ros::Subscriber sub = n_.subscribe( 59 | rt.c_str(), 1, 60 | boost::bind(&CollisionChecker::rangeCallback, this, _1)); 61 | range_subscribers_.push_back(sub); 62 | printf("%s. subscribed to topic: %s\n", 63 | ros::this_node::getName().c_str(), rt.c_str()); 64 | i++; 65 | } else 66 | ok = false; 67 | } 68 | num_ranges_ = (int)i; 69 | ranges_initiated_.assign(num_ranges_, false); 70 | // range_frames_.assign(num_ranges_, ""); 71 | } 72 | } 73 | 74 | void CollisionChecker::laserCallback( 75 | const sensor_msgs::LaserScan::ConstPtr &msg) { 76 | ROS_INFO_ONCE("CollisionChecker: Laser received!"); 77 | // IMPORTANT: the frame of the laser should be the center of the robot 78 | // (base_link) Otherwise we should include a shift to the center in the 79 | // calculations. 80 | 81 | std::vector points = laser_polar2euclidean(msg.get()); 82 | if (msg->header.frame_id != robot_frame_) 83 | transformPoints(points, msg->header.frame_id); 84 | 85 | laser_mutex_.lock(); 86 | scanpoints_ = points; 87 | laser_mutex_.unlock(); 88 | } 89 | 90 | std::vector 91 | CollisionChecker::laser_polar2euclidean(const sensor_msgs::LaserScan *scan) { 92 | std::vector points; 93 | points.reserve(scan->ranges.size()); 94 | geometry_msgs::Point p; 95 | p.z = 0.0; 96 | for (unsigned int i = 0; i < scan->ranges.size(); i++) { 97 | // laser measure polar coordinates 98 | float laser_d = scan->ranges[i]; 99 | float laser_th = scan->angle_min + scan->angle_increment * i; 100 | // transform to x,y 101 | p.x = laser_d * cos(laser_th); 102 | p.y = laser_d * sin(laser_th); 103 | points.push_back(p); 104 | } 105 | return points; 106 | } 107 | 108 | void CollisionChecker::transformPoints( 109 | std::vector &points, std::string input_frame) { 110 | 111 | // Transform points to 112 | // robot_frame_ if necessary 113 | geometry_msgs::PointStamped out; 114 | for (unsigned int i = 0; i < points.size(); i++) { 115 | geometry_msgs::PointStamped in; 116 | in.header.frame_id = input_frame; 117 | in.header.stamp = ros::Time(0); 118 | in.point.x = points[i].x; 119 | in.point.y = points[i].y; 120 | in.point.z = points[i].z; 121 | try { 122 | out = tf_->transform(in, robot_frame_); 123 | points[i].x = out.point.x; 124 | points[i].y = out.point.y; 125 | points[i].z = out.point.z; 126 | 127 | } catch (tf2::TransformException &ex) { 128 | ROS_WARN("[%s] Could NOT transform " 129 | "laser point to %s: " 130 | "%s", 131 | ros::this_node::getName(), robot_frame_.c_str(), ex.what()); 132 | return; 133 | } 134 | } 135 | } 136 | 137 | void CollisionChecker::rangeCallback(const sensor_msgs::Range::ConstPtr &msg) { 138 | ROS_INFO_ONCE( 139 | "Collision detector: range received! Detecting configuration..."); 140 | if (!ranges_ready_) { 141 | if (range_frames_.size() != (unsigned int)num_ranges_) { 142 | bool found = false; 143 | for (unsigned int i = 0; i < range_frames_.size(); i++) { 144 | if (range_frames_[i].compare(msg->header.frame_id) == 0) { 145 | found = true; 146 | break; 147 | } 148 | } 149 | if (!found) { 150 | range_frames_.push_back(msg->header.frame_id); 151 | 152 | range r; 153 | r.range = msg->range; 154 | r.id = msg->header.frame_id; 155 | r.min_dist = msg->min_range; 156 | r.max_dist = msg->max_range; 157 | r.fov = msg->field_of_view; 158 | // listen to the TF to know the position of the sonar range 159 | // tf::StampedTransform transform; 160 | geometry_msgs::TransformStamped ts; 161 | try { 162 | // tf_->waitForTransform(base_frame_.c_str(), r.id.c_str(), 163 | // ros::Time(0), 164 | // ros::Duration(3.0)); 165 | // tf_->lookupTransform(base_frame_.c_str(), r.id.c_str(), 166 | // ros::Time(0), transform); 167 | ts = tf_->lookupTransform(robot_frame_.c_str(), r.id.c_str(), 168 | ros::Time(0)); 169 | } catch (tf2::TransformException &ex) { 170 | ROS_ERROR("%s", ex.what()); 171 | } 172 | float x = ts.transform.translation.x; 173 | float y = ts.transform.translation.y; 174 | r.polar_dist = sqrt(x * x + y * y); 175 | // tf::Matrix3x3 m(ts.transform.rotation); 176 | // double roll, pitch, yaw; 177 | // m.getRPY(roll, pitch, yaw); 178 | tf2::Quaternion quat_tf; 179 | tf2::fromMsg(ts.transform.rotation, quat_tf); 180 | r.polar_angle = tf2::impl::getYaw(quat_tf); 181 | 182 | range_mutex_.lock(); 183 | ranges_.push_back(r); 184 | range_mutex_.unlock(); 185 | printf("Collision detector: Range %s configured.\n", 186 | msg->header.frame_id.c_str()); 187 | } 188 | } else { 189 | ranges_ready_ = true; 190 | printf("Collision detector: all the range sensors configured!!!\n\n"); 191 | } 192 | } else { 193 | range_mutex_.lock(); 194 | for (int i = 0; i < (int)ranges_.size(); i++) { 195 | if (ranges_[i].id.compare(msg->header.frame_id) == 0) { 196 | ranges_[i].range = msg->range; 197 | break; 198 | } 199 | } 200 | range_mutex_.unlock(); 201 | } 202 | } 203 | 204 | /* 205 | void CollisionChecker::saturateVelocities(geometry_msgs::Twist *twist) { 206 | float lv = twist->linear.x; 207 | float av = twist->angular.z; 208 | 209 | float rvx = robot_vel_.getOrigin().getX(); 210 | float rvy = robot_vel_.getOrigin().getY(); 211 | float rvt = tf::getYaw(robot_vel_.getRotation()); 212 | 213 | // acc linear 214 | if (fabs(rvx - lv) > max_lv_var_) { 215 | lv = (lv < rvx) ? (rvx - max_lv_var_) : (rvx + max_lv_var_); 216 | } 217 | // acc angular 218 | if (fabs(rvt - av) > max_av_var_) { 219 | av = (av < rvt) ? (rvt - max_av_var_) : (rvt + max_av_var_); 220 | } 221 | 222 | // Check maximum velocities 223 | if (lv > max_lin_vel_) 224 | lv = max_lin_vel_; 225 | else if (lv < (-max_lin_vel_)) 226 | lv = max_lin_vel_ * (-1); 227 | 228 | if (av > max_ang_vel_) 229 | av = max_ang_vel_; 230 | else if (av < (-max_ang_vel_)) 231 | av = max_ang_vel_ * (-1); 232 | 233 | twist->linear.x = lv; 234 | twist->angular.z = av; 235 | } 236 | */ 237 | 238 | bool CollisionChecker::inCollision( 239 | float x, float y, const std::vector &scanpoints) { 240 | if (use_range_ && inRangeCollision(x, y)) { 241 | printf("Possible collision detected!"); 242 | return true; 243 | } 244 | if (use_laser_ && inLaserCollision(x, y, scanpoints)) { 245 | return true; 246 | } 247 | return false; 248 | } 249 | 250 | bool CollisionChecker::inRangeCollision(float x, float y) { 251 | range_mutex_.lock(); 252 | for (unsigned int i = 0; i < ranges_.size(); i++) { 253 | // Main point 254 | float rx = (ranges_[i].polar_dist + ranges_[i].range) * 255 | cos(ranges_[i].polar_angle); 256 | float ry = (ranges_[i].polar_dist + ranges_[i].range) * 257 | sin(ranges_[i].polar_angle); 258 | float dx = (x - rx); 259 | float dy = (y - ry); 260 | float dist = dx * dx + dy * dy; 261 | if (dist <= (robot_radius_aug_ * robot_radius_aug_)) { 262 | ROS_INFO("POSSIBLE COLLISION DETECTED IN FRAME: %s", 263 | ranges_[i].id.c_str()); 264 | range_mutex_.unlock(); 265 | return true; 266 | } 267 | 268 | if (ranges_[i].fov > 0.2) { 269 | // second point 270 | rx = (ranges_[i].polar_dist + ranges_[i].range) * 271 | cos(ranges_[i].polar_angle + (ranges_[i].fov / 2.0)); 272 | ry = (ranges_[i].polar_dist + ranges_[i].range) * 273 | sin(ranges_[i].polar_angle + (ranges_[i].fov / 2.0)); 274 | dx = (x - rx); 275 | dy = (y - ry); 276 | dist = dx * dx + dy * dy; 277 | if (dist <= (robot_radius_aug_ * robot_radius_aug_)) { 278 | ROS_INFO("POSSIBLE COLLISION DETECTED (p2) IN FRAME: %s", 279 | ranges_[i].id.c_str()); 280 | range_mutex_.unlock(); 281 | return true; 282 | } 283 | 284 | // third point 285 | rx = (ranges_[i].polar_dist + ranges_[i].range) * 286 | cos(ranges_[i].polar_angle - (ranges_[i].fov / 2.0)); 287 | ry = (ranges_[i].polar_dist + ranges_[i].range) * 288 | sin(ranges_[i].polar_angle - (ranges_[i].fov / 2.0)); 289 | dx = (x - rx); 290 | dy = (y - ry); 291 | dist = dx * dx + dy * dy; 292 | if (dist <= (robot_radius_aug_ * robot_radius_aug_)) { 293 | ROS_INFO("POSSIBLE COLLISION DETECTED (p3) IN FRAME: %s", 294 | ranges_[i].id.c_str()); 295 | range_mutex_.unlock(); 296 | return true; 297 | } 298 | } 299 | } 300 | range_mutex_.unlock(); 301 | return false; 302 | } 303 | 304 | bool CollisionChecker::inLaserCollision( 305 | float x, float y, const std::vector &scanpoints) { 306 | for (unsigned int i = 0; i < scanpoints.size(); i++) { 307 | float dx = (x - scanpoints[i].x); 308 | float dy = (y - scanpoints[i].y); 309 | // float dist = sqrt(dx*dx + dy*dy); 310 | // if(dist <= robot_radius_) 311 | float dist = dx * dx + dy * dy; 312 | if (dist <= (robot_radius_aug_ * robot_radius_aug_)) 313 | return true; 314 | } 315 | return false; 316 | } 317 | 318 | /** 319 | * @brief Generate and check a single trajectory 320 | * @param cvx The current x velocity of the robot 321 | * @param cvy The current y velocity of the robot 322 | * @param cvth The current angular velocity of the robot 323 | * @param tvx The x velocity used to seed the trajectory 324 | * @param tvy The y velocity used to seed the trajectory 325 | * @param tvth The theta velocity used to seed the trajectory 326 | * @param dt The time for project the movement 327 | * @return True if the trajectory is legal, false otherwise 328 | */ 329 | bool CollisionChecker::checkCommand(double cvx, double cvy, double cvth, 330 | double tvx, double tvy, double tvth, 331 | double dt) { 332 | 333 | // printf("\nCollisionDetection. CheckCommand with vels vx:%.2f, vy:%.2f 334 | // th:%.2f\n", tvx, tvy, tvth); 335 | 336 | // double max_lv = computeNewVelocity(tvx, cvx, max_lin_acc_, sim_time_); 337 | // float vel_mag = sqrt(max_lv * max_lv); 338 | // float steps = (vel_mag * sim_time_) / granularity_; 339 | // float dt = sim_time_ / steps; 340 | float x = 0.0, y = 0.0, th = 0.0; 341 | 342 | double lv = cvx; 343 | double av = cvth; 344 | 345 | // geometry_msgs::PoseStamped pose; 346 | // pose.header.stamp = ros::Time(); // ros::Time::now(); 347 | // pose.header.frame_id = features_->getRobotBaseFrame(); // base_frame_; 348 | 349 | std::vector laser_points; 350 | if (use_laser_) { 351 | laser_mutex_.lock(); 352 | laser_points = scanpoints_; 353 | laser_mutex_.unlock(); 354 | } 355 | 356 | // if dt is higher than 0.1 secs, split it in steps 357 | params_mutex_.lock(); 358 | double step = 0.1; 359 | int steps = std::ceil(dt / step); 360 | for (unsigned int i = 0; i < steps; i++) { 361 | lv = computeNewVelocity(tvx, lv, max_lin_acc_, step); 362 | av = computeNewVelocity(tvth, av, max_ang_acc_, step); 363 | 364 | // x = computeNewXPosition(x, lv, 0.0, av, step); 365 | // y = computeNewYPosition(y, lv, 0.0, av, step); 366 | // th = computeNewThetaPosition(th, av, step); 367 | 368 | float lin_dist = lv * step; 369 | th = th + (av * step); 370 | // normalization just in case 371 | th = normalizeAngle(th, -M_PI, M_PI); 372 | x = x + lin_dist * cos(th); // cos(th+av*dt/2.0) 373 | y = y + lin_dist * sin(th); 374 | 375 | if (inCollision(x, y, laser_points)) { 376 | params_mutex_.unlock(); 377 | return false; 378 | } 379 | } 380 | 381 | // Now we check that we can stop without colliding 382 | while (lv > 0.0) { 383 | lv = computeNewVelocity(0.0, lv, max_lin_acc_, step); 384 | av = computeNewVelocity(0.0, av, max_ang_acc_, step); 385 | 386 | // x = computeNewXPosition(x, lv, 0.0, av, step); 387 | // y = computeNewYPosition(y, lv, 0.0, av, step); 388 | // th = computeNewThetaPosition(th, av, step); 389 | 390 | float lin_dist = lv * step; 391 | th = th + (av * step); 392 | // normalization just in case 393 | th = normalizeAngle(th, -M_PI, M_PI); 394 | x = x + lin_dist * cos(th); // cos(th+av*dt/2.0) 395 | y = y + lin_dist * sin(th); 396 | 397 | if (inCollision(x, y, laser_points)) { 398 | params_mutex_.unlock(); 399 | return false; 400 | } 401 | } 402 | params_mutex_.unlock(); 403 | return true; 404 | } 405 | 406 | } // namespace sfm_controller 407 | } // namespace Navigation 408 | } // namespace Upo 409 | -------------------------------------------------------------------------------- /src/sfm_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Local robot controller based on the Social Force Model 3 | * 4 | * Author: Noé Pérez-Higueras 5 | * Service Robotics Lab, Pablo de Olavide University 2021 6 | * 7 | * Software License Agreement (BSD License) 8 | * 9 | */ 10 | 11 | #include 12 | 13 | namespace Upo { 14 | namespace Navigation { 15 | namespace sfm_controller { 16 | 17 | /** 18 | * @brief Default constructor 19 | */ 20 | SFMController::SFMController() 21 | : max_lin_vel_(0.7), min_lin_vel_(0.1), max_vel_theta_(0.6), 22 | min_vel_theta_(0.1), min_rot_in_place_(0.4), max_lin_acc_(1.0), 23 | max_theta_acc_(1.0), local_goal_dist_(1.0), sfm_goal_weight_(2.0), 24 | sfm_obstacle_weight_(10.0), sfm_people_weight_(12.0), robot_radius_(0.35), 25 | person_radius_(0.35), goal_tolerance_(0.15), yaw_tolerance_(0.3), 26 | robot_frame_(std::string("base_link")), 27 | planner_frame_(std::string("odom")), a_(2.0) {} 28 | 29 | /** 30 | * @brief Constructor with parameters 31 | */ 32 | SFMController::SFMController( 33 | ros::NodeHandle *n, tf2_ros::Buffer *tfBuffer, 34 | SFMSensorInterface *sensor_iface, CollisionChecker *coll_check, 35 | float max_lin_vel, float min_lin_vel, float max_vel_theta, 36 | float min_vel_theta, float min_rot_in_place, float max_lin_acc, 37 | float max_theta_acc, float local_goal_dist, float sfm_goal_weight, 38 | float sfm_obstacle_weight, float sfm_people_weight, float robot_radius, 39 | float person_radius, float goal_tolerance, float yaw_tolerance, 40 | std::string robot_frame, std::string controller_frame, float a) 41 | : max_lin_vel_(max_lin_vel), min_lin_vel_(min_lin_vel), 42 | max_vel_theta_(max_vel_theta), min_vel_theta_(min_vel_theta), 43 | min_rot_in_place_(min_rot_in_place), max_lin_acc_(max_lin_acc), 44 | max_theta_acc_(max_theta_acc), local_goal_dist_(local_goal_dist), 45 | sfm_goal_weight_(sfm_goal_weight), 46 | sfm_obstacle_weight_(sfm_obstacle_weight), 47 | sfm_people_weight_(sfm_people_weight), robot_radius_(robot_radius), 48 | person_radius_(person_radius), goal_tolerance_(goal_tolerance), 49 | yaw_tolerance_(yaw_tolerance), robot_frame_(robot_frame), 50 | planner_frame_(controller_frame), a_(a) { 51 | // Initialize robot agent 52 | robot_.desiredVelocity = max_lin_vel_; 53 | robot_.radius = robot_radius_; 54 | robot_.cyclicGoals = false; 55 | robot_.teleoperated = true; 56 | robot_.params.forceFactorDesired = sfm_goal_weight_; 57 | robot_.params.forceFactorObstacle = sfm_obstacle_weight_; 58 | robot_.params.forceFactorSocial = sfm_people_weight_; 59 | 60 | std::cout << std::endl 61 | << "SFM_CONTROLLER INITIATED:" << std::endl 62 | << "max_lin_acc: " << max_lin_acc_ << std::endl 63 | << "max_rot_acc: " << max_theta_acc_ << std::endl 64 | << "min_lin_vel: " << min_lin_vel_ << std::endl 65 | << "max_lin_vel: " << max_lin_vel_ << std::endl 66 | << "min_rot_vel: " << min_vel_theta_ << std::endl 67 | << "max_rot_vel: " << max_vel_theta_ << std::endl 68 | << "local_goal_dist: " << local_goal_dist_ << std::endl 69 | << "goal_tolerance: " << goal_tolerance_ << std::endl 70 | << "robot_radius: " << robot_radius_ << std::endl 71 | << "person_radius: " << person_radius_ << std::endl 72 | << "robot_frame: " << robot_frame_ << std::endl 73 | << "planner_frame: " << planner_frame_ << std::endl 74 | << "sfm_goal_weight: " << sfm_goal_weight_ << std::endl 75 | << "sfm_obstacle_weight: " << sfm_obstacle_weight_ << std::endl 76 | << "sfm_social_weight: " << sfm_people_weight_ << std::endl 77 | << std::endl; 78 | 79 | // Advertise SFM related markers 80 | robot_markers_pub_ = n->advertise( 81 | "/sfm/markers/robot_forces", 1); 82 | 83 | // Adverstise SFM local goal 84 | sfm_goal_pub_ = 85 | n->advertise("/sfm/markers/goal", 1); 86 | 87 | // Initialize sensor interface 88 | sensor_iface_ = sensor_iface; 89 | 90 | // Initialize collision checker 91 | collision_checker_ = coll_check; 92 | 93 | last_command_time_ = ros::Time::now(); 94 | goal_reached_ = false; 95 | rotate_ = false; 96 | } 97 | 98 | /** 99 | * @brief Default destructor 100 | */ 101 | SFMController::~SFMController() { 102 | delete sensor_iface_; 103 | delete collision_checker_; 104 | } 105 | 106 | /** 107 | * @brief Callback to update the local planner's parameters based on dynamic 108 | * reconfigure 109 | */ 110 | void SFMController::reconfigure( 111 | sfm_local_controller::SFMLocalControllerConfig &cfg) { 112 | sfm_local_controller::SFMLocalControllerConfig config(cfg); 113 | 114 | configuration_mutex_.lock(); 115 | 116 | max_lin_acc_ = config.max_lin_acc; 117 | max_theta_acc_ = config.max_rot_acc; 118 | max_lin_vel_ = config.max_lin_vel; 119 | robot_.desiredVelocity = max_lin_vel_; 120 | min_lin_vel_ = config.min_lin_vel; 121 | max_vel_theta_ = config.max_rot_vel; 122 | min_vel_theta_ = config.min_rot_vel; 123 | 124 | sfm_goal_weight_ = config.sfm_goal_weight; 125 | sfm_obstacle_weight_ = config.sfm_obstacle_weight; 126 | sfm_people_weight_ = config.sfm_people_weight; 127 | robot_.params.forceFactorDesired = sfm_goal_weight_; 128 | robot_.params.forceFactorObstacle = sfm_obstacle_weight_; 129 | robot_.params.forceFactorSocial = sfm_people_weight_; 130 | 131 | // min_in_place_vel_th_ = config.min_in_place_rot_vel; 132 | // goal_lin_tolerance_ = config.xy_goal_tolerance; 133 | // goal_ang_tolerance_ = config.yaw_goal_tolerance; 134 | // wp_tolerance_ = config.wp_tolerance; 135 | 136 | sensor_iface_->setMaxLinVel(max_lin_vel_); 137 | collision_checker_->setVelParams(max_lin_vel_, max_vel_theta_, max_lin_acc_, 138 | max_theta_acc_); 139 | 140 | std::cout << std::endl 141 | << "SFM_CONTROLLER RECONFIGURED:" << std::endl 142 | << "max_lin_acc: " << max_lin_acc_ << std::endl 143 | << "max_rot_acc: " << max_theta_acc_ << std::endl 144 | << "min_lin_vel: " << min_lin_vel_ << std::endl 145 | << "max_lin_vel: " << max_lin_vel_ << std::endl 146 | << "min_rot_vel: " << min_vel_theta_ << std::endl 147 | << "max_rot_vel: " << max_vel_theta_ << std::endl 148 | << "sfm_goal_weight: " << sfm_goal_weight_ << std::endl 149 | << "sfm_obstacle_weight: " << sfm_obstacle_weight_ << std::endl 150 | << "sfm_social_weight: " << sfm_people_weight_ << std::endl 151 | << std::endl; 152 | 153 | configuration_mutex_.unlock(); 154 | } 155 | 156 | /** 157 | * @brief method to update the scenario situation 158 | * @param path global path to be followed by the local controller 159 | * @return True when the goal has been reached, False otherwise 160 | */ 161 | bool SFMController::update(std::vector path) { 162 | 163 | std::vector agents = sensor_iface_->getAgents(); 164 | 165 | configuration_mutex_.lock(); 166 | // update robot 167 | robot_.position = agents[0].position; 168 | robot_.yaw = agents[0].yaw; 169 | robot_.linearVelocity = agents[0].linearVelocity; 170 | robot_.angularVelocity = agents[0].angularVelocity; 171 | robot_.velocity = agents[0].velocity; 172 | robot_.obstacles1.clear(); 173 | robot_.obstacles1 = agents[0].obstacles1; 174 | 175 | // Update the rest of agents 176 | agents_.clear(); 177 | if (!agents.empty()) 178 | agents_.assign(++agents.begin(), agents.end()); 179 | 180 | // If we have to rotate, we do not look for a new goal 181 | if (rotate_) { 182 | sfm::Goal g; 183 | g.center.set(goal_.position.x, goal_.position.y); 184 | g.radius = goal_tolerance_; 185 | robot_.goals.clear(); 186 | robot_.goals.push_back(g); 187 | if (goal_reached_) { 188 | goal_reached_ = false; 189 | rotate_ = false; 190 | configuration_mutex_.unlock(); 191 | return true; 192 | } 193 | configuration_mutex_.unlock(); 194 | return false; 195 | } 196 | 197 | // Update the robot goal 198 | geometry_msgs::PoseStamped min; 199 | float min_dist = 9999.0; 200 | bool goal_found = false; 201 | for (unsigned int i = path.size() - 1; i > 0; i--) { 202 | float dx = path[i].pose.position.x - robot_.position.getX(); 203 | float dy = path[i].pose.position.y - robot_.position.getY(); 204 | float d = sqrt(dx * dx + dy * dy); 205 | if (d < min_dist) { 206 | min_dist = d; 207 | min = path[i]; 208 | } 209 | if (d <= local_goal_dist_) { 210 | if (d < goal_tolerance_) { 211 | configuration_mutex_.unlock(); 212 | // goal reached! - rotate! 213 | printf("Update. Goal location reached. Rotating...\n"); 214 | rotate_ = true; 215 | return false; 216 | } 217 | sfm::Goal g; 218 | goal_ = path[i].pose; 219 | g.center.set(path[i].pose.position.x, path[i].pose.position.y); 220 | g.radius = goal_tolerance_; 221 | robot_.goals.clear(); 222 | robot_.goals.push_back(g); 223 | goal_found = true; 224 | goal_reached_ = false; 225 | // printf("Update. Goal found! x: %.2f, y: %.2f\n", g.center.getX(), 226 | // g.center.getY()); 227 | publishSFMGoal(path[i]); 228 | configuration_mutex_.unlock(); 229 | return false; 230 | } 231 | } 232 | // printf("Goal not found closer than %.2f,\n", local_goal_dist_); 233 | // printf("the closest is %.2f m\n", min_dist); 234 | if (!path.empty()) { 235 | sfm::Goal g; 236 | goal_ = min.pose; 237 | g.center.set(min.pose.position.x, min.pose.position.y); 238 | g.radius = goal_tolerance_; 239 | robot_.goals.clear(); 240 | robot_.goals.push_back(g); 241 | goal_reached_ = false; 242 | publishSFMGoal(min); 243 | } else { 244 | printf("Update. Goal not found. Received path size: %i\n", 245 | (int)path.size()); 246 | } 247 | configuration_mutex_.unlock(); 248 | return false; 249 | } 250 | 251 | /** 252 | * @brief method to compute the velocity command of the robot 253 | * @param cmd_vel velocity message to be filled 254 | * @return True if a command vel was found 255 | */ 256 | bool SFMController::computeAction(geometry_msgs::Twist &cmd_vel) { 257 | 258 | configuration_mutex_.lock(); 259 | double dt = (ros::Time::now() - last_command_time_).toSec(); 260 | // printf("dt: %.4f\n", dt); 261 | if (dt > 0.2) 262 | dt = 0.1; 263 | 264 | last_command_time_ = ros::Time::now(); 265 | 266 | // We must rotate to reach the goal position 267 | if (rotate_) { 268 | float ang_diff = robot_.yaw.toRadian() - tf::getYaw(goal_.orientation); 269 | ang_diff = normalizeAngle(ang_diff, -M_PI, M_PI); 270 | if (fabs(ang_diff) < yaw_tolerance_) { 271 | printf("Angdiff (%.2f) < yaw_tolerance (%.2f)\n", fabs(ang_diff), 272 | yaw_tolerance_); 273 | cmd_vel.linear.x = 0.0; 274 | cmd_vel.linear.y = 0.0; 275 | cmd_vel.linear.z = 0.0; 276 | cmd_vel.angular.x = 0.0; 277 | cmd_vel.angular.y = 0.0; 278 | cmd_vel.angular.z = 0.0; 279 | goal_reached_ = true; 280 | configuration_mutex_.unlock(); 281 | return true; 282 | } else if (ang_diff > 0.0) { 283 | cmd_vel.angular.z = -min_rot_in_place_; 284 | // printf("Rotating with vel: %.2f\n", cmd_vel.angular.z); 285 | } else { 286 | cmd_vel.angular.z = min_rot_in_place_; 287 | // printf("Rotating with vel: %.2f\n", cmd_vel.angular.z); 288 | } 289 | configuration_mutex_.unlock(); 290 | return true; 291 | } 292 | 293 | // Compute Social Forces 294 | sfm::SFM.computeForces(robot_, agents_); 295 | 296 | // Compute velocity of the robot 297 | robot_.velocity += robot_.forces.globalForce * dt; 298 | if (robot_.velocity.norm() > robot_.desiredVelocity) { 299 | robot_.velocity.normalize(); 300 | robot_.velocity *= robot_.desiredVelocity; 301 | } 302 | 303 | // The resultant total velocity is expressed in the odom frame. Transform 304 | // to robot_frame 305 | geometry_msgs::Vector3 velocity; 306 | velocity.x = robot_.velocity.getX(); 307 | velocity.y = robot_.velocity.getY(); 308 | geometry_msgs::Vector3 localV = 309 | sensor_iface_->transformVector(velocity, planner_frame_, robot_frame_); 310 | 311 | utils::Vector2d vel; 312 | vel.set(localV.x, localV.y); 313 | cmd_vel.linear.x = vel.norm(); 314 | 315 | // Decrease speed to approach the goal 316 | float dx = goal_.position.x - robot_.position.getX(); 317 | float dy = goal_.position.y - robot_.position.getY(); 318 | float d = sqrt(dx * dx + dy * dy); 319 | if (d < 1.0) 320 | cmd_vel.linear.x = 321 | (vel.norm() * d) < min_lin_vel_ ? min_lin_vel_ : (vel.norm() * d); 322 | 323 | cmd_vel.linear.y = 0.0; 324 | cmd_vel.linear.z = 0.0; 325 | // Wz = std::atan2(localV.y,localV.x)/2.0; 326 | double angle = std::atan2(localV.y, localV.x); 327 | cmd_vel.angular.z = getVel(max_vel_theta_, a_, angle); 328 | cmd_vel.angular.x = 0.0; 329 | cmd_vel.angular.y = 0.0; 330 | 331 | // Prevent the robot for turning around: 332 | // If the angle difference between the desired force 333 | // and the global force is almost opossite, 334 | // stop the robot instead of turning around 335 | double angle_deg = 336 | robot_.forces.desiredForce.angleTo(robot_.forces.globalForce).toDegree(); 337 | if ((180.0 - fabs(angle_deg)) < 25.0) { 338 | // printf("\nStopping robot. angle_deg: %.3f!!!!!\n", angle_deg); 339 | cmd_vel.linear.x = 0.0; 340 | cmd_vel.angular.z = 0.0; 341 | } 342 | 343 | publishForces(); 344 | if (!collision_checker_->checkCommand( 345 | robot_.linearVelocity, 0.0, robot_.angularVelocity, cmd_vel.linear.x, 346 | 0.0, cmd_vel.angular.z, 0.11)) { 347 | ROS_WARN("Possible collision detected! Sending zero vel!"); 348 | cmd_vel.linear.x = 0.0; 349 | cmd_vel.angular.z = 0.0; 350 | } 351 | 352 | // ROS_INFO("LV: %f; AV: %f", cmd_vel.linear.x, cmd_vel.angular.z); 353 | configuration_mutex_.unlock(); 354 | return true; 355 | } 356 | 357 | /** 358 | * @brief check if the current scenario leads to a possible collision 359 | * @return True if a possible collision was detected, False otherwise 360 | */ 361 | bool SFMController::fastCollisioncheck() { 362 | // return sensor_iface_->collisionCheck(); 363 | return true; 364 | } 365 | 366 | /** 367 | * @brief Publish in RViz the local goal followed by the controller 368 | * @param g goal position 369 | * @return none 370 | */ 371 | void SFMController::publishSFMGoal(const geometry_msgs::PoseStamped &g) { 372 | visualization_msgs::Marker marker; 373 | marker.header.frame_id = g.header.frame_id; 374 | marker.header.stamp = ros::Time::now(); 375 | marker.ns = "sfm_goal"; 376 | marker.id = 1; 377 | marker.action = visualization_msgs::Marker::ADD; 378 | marker.type = visualization_msgs::Marker::ARROW; 379 | marker.color = getColor(0.0, 0.0, 1.0, 1.0); 380 | marker.lifetime = ros::Duration(0.15); 381 | marker.scale.x = 0.4; 382 | marker.scale.y = 0.1; 383 | marker.scale.z = 0.1; 384 | marker.pose = g.pose; 385 | sfm_goal_pub_.publish(marker); 386 | } 387 | 388 | /** 389 | * @brief Publish an arrow marker in Rviz representing a force 390 | * @param index id of the marker 391 | * @param color RGB color of the marker 392 | * @param force force to be represented 393 | * @param markers markerArray in which the arrow will be added 394 | * @return none 395 | */ 396 | void SFMController::publishForceMarker( 397 | unsigned index, const std_msgs::ColorRGBA &color, 398 | const utils::Vector2d &force, visualization_msgs::MarkerArray &markers) { 399 | visualization_msgs::Marker marker; 400 | marker.header.frame_id = planner_frame_; 401 | marker.header.stamp = ros::Time::now(); 402 | marker.ns = "robot_forces"; 403 | marker.id = index; 404 | marker.action = force.norm() > 1e-4 ? 0 : 2; 405 | marker.color = color; 406 | marker.lifetime = ros::Duration(1.0); 407 | marker.scale.x = std::max(1e-4, force.norm()); 408 | marker.scale.y = 0.1; 409 | marker.scale.z = 0.1; 410 | marker.pose.position.x = robot_.position.getX(); 411 | marker.pose.position.y = robot_.position.getY(); 412 | marker.pose.position.z = 0; 413 | marker.pose.orientation = 414 | tf::createQuaternionMsgFromRollPitchYaw(0, 0, force.angle().toRadian()); 415 | markers.markers.push_back(marker); 416 | } 417 | 418 | /** 419 | * @brief fill a ColorRGBA message 420 | * @param r value of the red componet [0-1] 421 | * @param g value of the green component [0-1] 422 | * @param b value of the blue component [0-1] 423 | * @param a transparency of the color [0-1] 424 | * @return a ROS ColorRGBA message 425 | */ 426 | std_msgs::ColorRGBA SFMController::getColor(double r, double g, double b, 427 | double a) { 428 | std_msgs::ColorRGBA color; 429 | color.r = r; 430 | color.g = g; 431 | color.b = b; 432 | color.a = a; 433 | return color; 434 | } 435 | 436 | /** 437 | * @brief Publish the set of SFM forces in RViz 438 | * @return none 439 | */ 440 | void SFMController::publishForces() { 441 | visualization_msgs::MarkerArray markers; 442 | publishForceMarker(0, getColor(1, 0, 0, 1), robot_.forces.obstacleForce, 443 | markers); 444 | publishForceMarker(1, getColor(0, 0, 1, 1), robot_.forces.socialForce, 445 | markers); 446 | // publishForceMarker(2, getColor(0, 1, 1, 1), robot_.forces.groupForce, 447 | // markers); 448 | publishForceMarker(3, getColor(0, 1, 0, 1), robot_.forces.desiredForce, 449 | markers); 450 | publishForceMarker(4, getColor(1, 1, 1, 1), robot_.forces.globalForce, 451 | markers); 452 | publishForceMarker(5, getColor(1, 1, 0, 1), robot_.velocity, markers); 453 | robot_markers_pub_.publish(markers); 454 | // ROS_INFO_STREAM("Goal: " << robot_.forces.desiredForce.norm() 455 | // << ", Obstacle: " 456 | // << robot_.forces.obstacleForce.norm() << ", People: 457 | // " 458 | // << robot_.forces.socialForce.norm() << std::endl); 459 | } 460 | 461 | } // namespace sfm_controller 462 | } // namespace Navigation 463 | } // namespace Upo -------------------------------------------------------------------------------- /src/sensor_interface.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Class to adapt the sensory input and detections to be used by the 3 | * sfm_local_controller 4 | * 5 | * Author: Noé Pérez-Higueras 6 | * Service Robotics Lab, Pablo de Olavide University 2021 7 | * 8 | * Software License Agreement (BSD License) 9 | * 10 | */ 11 | 12 | #include 13 | 14 | namespace Upo { 15 | namespace Navigation { 16 | namespace sfm_controller { 17 | 18 | /** 19 | * @brief Default constructor 20 | * @param n pointer to a ros node handle to publish to topics 21 | * @param tfBuffer Pointer to tf2 buffer 22 | * @param robot_max_lin_speed the maximum robot linear speed [m/s] 23 | * @param robot_radius the radius of the robot circunference [m] 24 | * @param person_radius the approximated radius of the people body 25 | * @param robot_frame the coordinate frame of the robot base 26 | * @param odom_frame the coordinate frame of the robot odometry 27 | **/ 28 | SFMSensorInterface::SFMSensorInterface(ros::NodeHandle *n, 29 | tf2_ros::Buffer *tfBuffer, 30 | float robot_max_lin_speed, 31 | float robot_radius, float person_radius, 32 | std::string robot_frame, 33 | std::string odom_frame) 34 | : nh_(n), n_(), tf_buffer_(tfBuffer), person_radius_(person_radius), 35 | robot_frame_(robot_frame), odom_frame_(odom_frame) { 36 | 37 | nh_->param("max_obstacle_dist", max_obstacle_dist_, float(3.0)); 38 | nh_->param("naive_goal_time", naive_goal_time_, float(2.0)); 39 | nh_->param("people_velocity", people_velocity_, float(1.0)); 40 | 41 | laser_received_ = false; 42 | last_laser_ = ros::Time::now(); 43 | 44 | std::string laser_topic; 45 | nh_->param("laser_topic", laser_topic, std::string("scan")); 46 | std::string people_topic; 47 | nh_->param("people_topic", people_topic, std::string("people")); 48 | std::string obs_topic; 49 | nh_->param("dyn_obs_topic", obs_topic, std::string("obstacles")); 50 | std::string odom_topic; 51 | nh_->param("odom_topic", odom_topic, std::string("odom")); 52 | 53 | std::cout << std::endl 54 | << "SFM SENSOR INTERFACE:" << std::endl 55 | << "laser_topic: " << laser_topic << std::endl 56 | << "people_topic: " << people_topic << std::endl 57 | << "dyn_obs_topic: " << obs_topic << std::endl 58 | << "odom_topic: " << odom_topic << std::endl 59 | << "max_obstacle_dist: " << max_obstacle_dist_ << std::endl 60 | << "naive_goal_time: " << naive_goal_time_ << std::endl 61 | << "people_velocity: " << people_velocity_ << std::endl 62 | << std::endl; 63 | 64 | // Initialize SFM. Just one agent (the robot) 65 | agents_.resize(1); 66 | agents_[0].desiredVelocity = robot_max_lin_speed; 67 | agents_[0].radius = robot_radius; 68 | agents_[0].cyclicGoals = false; 69 | agents_[0].teleoperated = true; 70 | agents_[0].groupId = -1; 71 | 72 | // ros::NodeHandle n; 73 | laser_sub_ = n_.subscribe( 74 | laser_topic.c_str(), 1, &SFMSensorInterface::laserCb, this); 75 | 76 | people_sub_ = n_.subscribe( 77 | people_topic.c_str(), 1, &SFMSensorInterface::peopleCb, this); 78 | 79 | dyn_obs_sub_ = n_.subscribe( 80 | obs_topic.c_str(), 1, &SFMSensorInterface::dynamicObsCb, this); 81 | 82 | odom_sub_ = n_.subscribe( 83 | odom_topic.c_str(), 1, &SFMSensorInterface::odomCb, this); 84 | 85 | points_pub_ = n_.advertise( 86 | "/sfm/markers/obstacle_points", 0); 87 | 88 | // sonars ? 89 | // sonar_sub_= n_.subscribe( 90 | // rt.c_str(), 1, boost::bind(&SFMSensorInterface::sonarCb, this, _1)); 91 | } 92 | 93 | /** 94 | * @brief Destructor class 95 | */ 96 | SFMSensorInterface::~SFMSensorInterface() {} 97 | 98 | /** 99 | * @brief Callback to process the laser scan sensory input. 100 | * @param laser laserScan message to be processed 101 | */ 102 | void SFMSensorInterface::laserCb( 103 | const sensor_msgs::LaserScan::ConstPtr &laser) { 104 | 105 | laser_received_ = true; 106 | last_laser_ = ros::Time::now(); 107 | 108 | ROS_INFO_ONCE("laser received"); 109 | 110 | std::vector points; 111 | float angle = laser->angle_min; 112 | 113 | for (unsigned int i = 0; i < laser->ranges.size(); i++) { 114 | 115 | if (laser->ranges[i] < max_obstacle_dist_) { 116 | 117 | utils::Vector2d point(laser->ranges[i] * cos(angle), 118 | laser->ranges[i] * sin(angle)); 119 | points.push_back(point); 120 | } 121 | angle += laser->angle_increment; 122 | // alpha += angle_inc; 123 | } 124 | 125 | if (points.empty()) { 126 | obstacles_ = points; 127 | return; 128 | } 129 | 130 | // Transform points to 131 | // odom_frame_ if necessary 132 | if (laser->header.frame_id != odom_frame_) { 133 | geometry_msgs::PointStamped out; 134 | for (unsigned int i = 0; i < points.size(); i++) { 135 | geometry_msgs::PointStamped in; 136 | in.header.frame_id = laser->header.frame_id; 137 | in.header.stamp = ros::Time(0); 138 | in.point.x = points[i].getX(); 139 | in.point.y = points[i].getY(); 140 | in.point.z = 0.0; 141 | try { 142 | geometry_msgs::PointStamped out = 143 | tf_buffer_->transform(in, odom_frame_); 144 | points[i].setX(out.point.x); 145 | points[i].setY(out.point.y); 146 | 147 | } catch (tf2::TransformException &ex) { 148 | ROS_WARN("Could NOT transform " 149 | "laser point to %s: " 150 | "%s", 151 | robot_frame_.c_str(), ex.what()); 152 | return; 153 | } 154 | } 155 | } 156 | 157 | // Now check if the points 158 | // belong to a person or an 159 | // dynamic obstacle 160 | 161 | // transform people positions to 162 | // robot frame 163 | // people_mutex_.lock(); 164 | people_msgs::People people = people_; 165 | // people_mutex_.unlock(); 166 | 167 | std::vector people_points; 168 | if (people.header.frame_id != odom_frame_) { 169 | geometry_msgs::PointStamped person_point; 170 | person_point.header = people.header; 171 | 172 | for (auto person : people.people) { 173 | person_point.point = person.position; 174 | person_point.header.stamp = ros::Time(0); 175 | try { 176 | geometry_msgs::PointStamped p_point = 177 | tf_buffer_->transform(person_point, odom_frame_); 178 | people_points.push_back(p_point.point); 179 | } catch (tf2::TransformException &ex) { 180 | ROS_WARN("Could NOT transform " 181 | "person point to %s: " 182 | "%s", 183 | odom_frame_.c_str(), ex.what()); 184 | return; 185 | } 186 | } 187 | } else { 188 | for (auto person : people.people) { 189 | people_points.push_back(person.position); 190 | } 191 | } 192 | // Remove the points in the 193 | // people radius 194 | std::vector points_aux; 195 | for (utils::Vector2d p : points) { 196 | bool remove = false; 197 | for (auto person : people_points) { 198 | float dx = p.getX() - person.x; 199 | float dy = p.getY() - person.y; 200 | float d = std::hypotf(dx, dy); 201 | if (d <= person_radius_) { 202 | remove = true; 203 | break; 204 | } 205 | } 206 | if (!remove) 207 | points_aux.push_back(p); 208 | } 209 | points.clear(); 210 | points = points_aux; 211 | // we can have one point per 212 | // sector as much 213 | if (points.empty()) { 214 | obstacles_ = points; 215 | return; 216 | } 217 | 218 | // transform dynamic obstacles 219 | // positions to robot frame 220 | // obs_mutex_.lock(); 221 | dynamic_obstacle_detector::DynamicObstacles obstacles = dyn_obs_; 222 | // obs_mutex_.unlock(); 223 | 224 | std::vector ob_points; 225 | if (obstacles.header.frame_id != odom_frame_) { 226 | geometry_msgs::PointStamped ob_point; 227 | ob_point.header = obstacles.header; 228 | // ob_point.stamp = ros::Time(); 229 | for (auto obstacle : obstacles.obstacles) { 230 | ob_point.point = obstacle.position; 231 | ob_point.header.stamp = ros::Time(0); 232 | try { 233 | geometry_msgs::PointStamped o_point = 234 | tf_buffer_->transform(ob_point, odom_frame_); 235 | ob_points.push_back(o_point.point); 236 | } catch (tf2::TransformException &ex) { 237 | ROS_WARN("Could NOT transform " 238 | "obstacle point to %s: " 239 | "%s", 240 | odom_frame_.c_str(), ex.what()); 241 | return; 242 | } 243 | } 244 | } 245 | // Remove the points in the 246 | // object radius (approximated 247 | // by person radius because we 248 | // do not know the radius) 249 | points_aux.clear(); 250 | for (utils::Vector2d p : points) { 251 | bool remove = false; 252 | for (auto ob : ob_points) { 253 | float dx = p.getX() - ob.x; 254 | float dy = p.getY() - ob.y; 255 | float d = std::hypotf(dx, dy); 256 | if (d <= person_radius_) { 257 | remove = true; 258 | break; 259 | } 260 | } 261 | if (!remove) 262 | points_aux.push_back(p); 263 | } 264 | points.clear(); 265 | points = points_aux; 266 | 267 | obstacles_ = points; 268 | publish_obstacle_points(points); 269 | } 270 | 271 | /** 272 | * @brief publish the transformed points in RViz 273 | * @param points vector with the coordinates of the points 274 | * @return none 275 | */ 276 | void SFMSensorInterface::publish_obstacle_points( 277 | const std::vector &points) { 278 | 279 | visualization_msgs::Marker m; 280 | m.header.frame_id = odom_frame_; // robot_frame_ 281 | m.header.stamp = ros::Time::now(); 282 | m.ns = "sfm_obstacle_points"; 283 | m.type = visualization_msgs::Marker::POINTS; 284 | m.action = visualization_msgs::Marker::ADD; 285 | m.pose.orientation.x = 0.0; 286 | m.pose.orientation.y = 0.0; 287 | m.pose.orientation.z = 0.0; 288 | m.pose.orientation.w = 1.0; 289 | m.scale.x = 0.15; 290 | m.scale.y = 0.15; 291 | m.scale.z = 0.15; 292 | m.color.r = 1.0; 293 | m.color.g = 0.0; 294 | m.color.b = 0.0; 295 | m.color.a = 1.0; 296 | m.id = 1000; 297 | m.lifetime = ros::Duration(0.3); 298 | // printf("Published Obstacles: "); 299 | for (utils::Vector2d p : points) { 300 | geometry_msgs::Point pt; 301 | pt.x = p.getX(); 302 | pt.y = p.getY(); 303 | // printf("x: %.2f, y: %.2f -", pt.x, pt.y); 304 | pt.z = 0.2; 305 | m.points.push_back(pt); 306 | } 307 | // printf("\n"); 308 | points_pub_.publish(m); 309 | } 310 | 311 | /** 312 | * @brief Callback to process the moving obstacles detected in the robot 313 | * vecinity. 314 | * @param obs messages with the obstacles to be processed. 315 | */ 316 | void SFMSensorInterface::dynamicObsCb( 317 | const dynamic_obstacle_detector::DynamicObstacles::ConstPtr &obs) { 318 | ROS_INFO_ONCE("Dyamic obs received"); 319 | 320 | // obs_mutex_.lock(); 321 | dyn_obs_ = *obs; 322 | // obs_mutex_.unlock(); 323 | 324 | std::vector agents; 325 | 326 | // check if people are not in odom frame 327 | geometry_msgs::PointStamped ps; 328 | for (unsigned i = 0; i < obs->obstacles.size(); i++) { 329 | sfm::Agent ag; 330 | ps.header.frame_id = obs->header.frame_id; 331 | ps.header.stamp = ros::Time(0); 332 | ps.point = obs->obstacles[i].position; 333 | if (obs->header.frame_id != odom_frame_) { 334 | geometry_msgs::PointStamped p; 335 | try { 336 | p = tf_buffer_->transform(ps, odom_frame_); 337 | ps = p; 338 | } catch (tf2::TransformException &ex) { 339 | ROS_WARN("No transform %s", ex.what()); 340 | } 341 | } 342 | ag.position.set(ps.point.x, ps.point.y); 343 | 344 | geometry_msgs::Vector3 velocity; 345 | velocity.x = obs->obstacles[i].velocity.x; 346 | velocity.y = obs->obstacles[i].velocity.y; 347 | 348 | geometry_msgs::Vector3 localV = SFMSensorInterface::transformVector( 349 | velocity, obs->header.frame_id, odom_frame_); 350 | 351 | ag.yaw = utils::Angle::fromRadian(atan2(localV.y, localV.x)); 352 | ag.velocity.set(localV.x, localV.y); 353 | ag.linearVelocity = ag.velocity.norm(); 354 | ag.radius = person_radius_; 355 | ag.teleoperated = false; 356 | // if (fabs(people->people[i].vel) < 0.05) { 357 | // agents[i+1].velocity.set(0,0); 358 | //} 359 | 360 | // The SFM requires a local goal for each agent. We will assume that the 361 | // goal for people depends on its current velocity 362 | ag.goals.clear(); 363 | sfm::Goal naiveGoal; 364 | // No group consideration for the moment 365 | utils::Vector2d v = ag.position + naive_goal_time_ * ag.velocity; 366 | naiveGoal.center.set(v.getX(), v.getY()); 367 | naiveGoal.radius = person_radius_; 368 | ag.goals.push_back(naiveGoal); 369 | ag.desiredVelocity = people_velocity_; 370 | ag.groupId = -1; 371 | agents.push_back(ag); 372 | } 373 | 374 | // Fill the obstacles of the agents 375 | std::vector obs_points = obstacles_; 376 | for (unsigned int i = 0; i < agents.size(); i++) { 377 | agents[i].obstacles1.clear(); 378 | agents[i].obstacles1 = obs_points; 379 | } 380 | 381 | agents_mutex_.lock(); 382 | agents_.resize(obs->obstacles.size() + 1); 383 | agents_[0].obstacles1 = obs_points; 384 | for (unsigned int i = 1; i < agents_.size(); i++) 385 | agents_[i] = agents[i - 1]; 386 | agents_mutex_.unlock(); 387 | } 388 | 389 | /** 390 | * @brief Callback to process the people detected in the robot vecinity. 391 | * @param people message with the people to be processed 392 | */ 393 | void SFMSensorInterface::peopleCb(const people_msgs::People::ConstPtr &people) { 394 | ROS_INFO_ONCE("People received"); 395 | 396 | // people_mutex_.lock(); 397 | people_ = *people; 398 | // people_mutex_.unlock(); 399 | 400 | std::vector agents; 401 | 402 | // check if people are not in odom frame 403 | geometry_msgs::PointStamped ps; 404 | for (unsigned i = 0; i < people->people.size(); i++) { 405 | sfm::Agent ag; 406 | ps.header.frame_id = people->header.frame_id; 407 | ps.header.stamp = ros::Time(0); 408 | ps.point = people->people[i].position; 409 | if (people->header.frame_id != odom_frame_) { 410 | geometry_msgs::PointStamped p; 411 | try { 412 | p = tf_buffer_->transform(ps, odom_frame_); 413 | ps = p; 414 | } catch (tf2::TransformException &ex) { 415 | ROS_WARN("No transform %s", ex.what()); 416 | } 417 | } 418 | ag.position.set(ps.point.x, ps.point.y); 419 | 420 | geometry_msgs::Vector3 velocity; 421 | velocity.x = people->people[i].velocity.x; 422 | velocity.y = people->people[i].velocity.y; 423 | 424 | geometry_msgs::Vector3 localV = SFMSensorInterface::transformVector( 425 | velocity, people->header.frame_id, odom_frame_); 426 | 427 | ag.yaw = utils::Angle::fromRadian(atan2(localV.y, localV.x)); 428 | ag.velocity.set(localV.x, localV.y); 429 | ag.linearVelocity = agents_[i + 1].velocity.norm(); 430 | ag.radius = person_radius_; 431 | ag.teleoperated = false; 432 | // if (fabs(people->people[i].vel) < 0.05) { 433 | // agents[i+1].velocity.set(0,0); 434 | //} 435 | 436 | // The SFM requires a local goal for each agent. We will assume that the 437 | // goal for people depends on its current velocity 438 | ag.goals.clear(); 439 | sfm::Goal naiveGoal; 440 | // No group consideration for the moment 441 | // naiveGoal.center = 442 | // agents_[i + 1].position + naive_goal_time_ * agents_[i + 1].velocity; 443 | utils::Vector2d v = ag.position + naive_goal_time_ * ag.velocity; 444 | naiveGoal.center.set(v.getX(), v.getY()); 445 | naiveGoal.radius = person_radius_; 446 | ag.goals.push_back(naiveGoal); 447 | ag.desiredVelocity = people_velocity_; 448 | ag.groupId = -1; 449 | agents.push_back(ag); 450 | } 451 | 452 | // Fill the obstacles of the agents 453 | std::vector obs_points = obstacles_; 454 | for (unsigned int i = 0; i < agents.size(); i++) { 455 | agents[i].obstacles1.clear(); 456 | agents[i].obstacles1 = obs_points; 457 | } 458 | 459 | agents_mutex_.lock(); 460 | agents_.resize(people->people.size() + 1); 461 | agents_[0].obstacles1 = obs_points; 462 | for (unsigned int i = 1; i < agents_.size(); i++) 463 | agents_[i] = agents[i - 1]; 464 | agents_mutex_.unlock(); 465 | } 466 | 467 | /** 468 | * @brief Callback to process the odometry messages with the robot movement. 469 | * @param odom messages with the obstacles to be processed. 470 | */ 471 | void SFMSensorInterface::odomCb(const nav_msgs::Odometry::ConstPtr &odom) { 472 | ROS_INFO_ONCE("Odom received"); 473 | 474 | // if (odom->header.frame_id != odom_frame_) { 475 | // ROS_INFO("Odometry frame is %s, it should be %s", 476 | // odom->header.frame_id.c_str(), odom_frame_.c_str()); 477 | // // return; 478 | // } 479 | 480 | agents_mutex_.lock(); 481 | sfm::Agent agent = agents_[0]; 482 | agents_mutex_.unlock(); 483 | 484 | agent.position.set(odom->pose.pose.position.x, odom->pose.pose.position.y); 485 | agent.yaw = utils::Angle::fromRadian(tf::getYaw(odom->pose.pose.orientation)); 486 | 487 | agent.linearVelocity = 488 | std::sqrt(odom->twist.twist.linear.x * odom->twist.twist.linear.x + 489 | odom->twist.twist.linear.y * odom->twist.twist.linear.y); 490 | agent.angularVelocity = odom->twist.twist.angular.z; 491 | 492 | // The velocity in the odom messages is in the robot local frame!!! 493 | geometry_msgs::Vector3 velocity; 494 | velocity.x = odom->twist.twist.linear.x; 495 | velocity.y = odom->twist.twist.linear.y; 496 | 497 | geometry_msgs::Vector3 localV = 498 | SFMSensorInterface::transformVector(velocity, robot_frame_, odom_frame_); 499 | agent.velocity.set(localV.x, localV.y); 500 | 501 | // Update agent[0] (the robot) with odom. 502 | agents_mutex_.lock(); 503 | agents_[0] = agent; 504 | agents_mutex_.unlock(); 505 | } 506 | 507 | // void SFMSensorInterface::updateAgents() { 508 | // Get odom 509 | //} 510 | 511 | /** 512 | * @brief returns the vector of sfm agents 513 | * @return agents vector 514 | */ 515 | std::vector SFMSensorInterface::getAgents() { 516 | agents_mutex_.lock(); 517 | std::vector agents = agents_; 518 | agents_mutex_.unlock(); 519 | return agents; 520 | } 521 | 522 | /** 523 | * @brief Tranform a coordinate vector from one frame to another 524 | * @param vector coordinate vector in the origin frame 525 | * @param from string with the name of the origin frame 526 | * @param to string with the name of the target frame 527 | * @return coordinate vector in the target frame 528 | */ 529 | geometry_msgs::Vector3 530 | SFMSensorInterface::transformVector(geometry_msgs::Vector3 &vector, 531 | std::string from, std::string to) { 532 | geometry_msgs::Vector3 nextVector; 533 | 534 | geometry_msgs::Vector3Stamped v; 535 | geometry_msgs::Vector3Stamped nv; 536 | 537 | v.header.frame_id = from; 538 | 539 | // we'll just use the most recent transform available for our simple example 540 | v.header.stamp = ros::Time(0); 541 | 542 | // just an arbitrary point in space 543 | v.vector.x = vector.x; 544 | v.vector.y = vector.y; 545 | v.vector.z = 0.0; 546 | 547 | try { 548 | nv = tf_buffer_->transform(v, to); 549 | } catch (tf2::TransformException &ex) { 550 | ROS_WARN("No transform %s", ex.what()); 551 | } 552 | 553 | nextVector.x = nv.vector.x; 554 | nextVector.y = nv.vector.y; 555 | nextVector.z = 0.0; 556 | 557 | return nextVector; 558 | } 559 | 560 | } // namespace sfm_controller 561 | } // namespace Navigation 562 | } // namespace Upo --------------------------------------------------------------------------------