├── maps ├── map.pgm ├── house.pgm ├── house_amdp.bmp ├── house_amdp.pgm ├── house.yaml ├── map.yaml ├── house_amdp.yaml └── cost.yaml ├── action └── Vi.action ├── src ├── SweepWorkerStatus.cpp ├── Action.cpp ├── StateTransition.cpp ├── State.cpp ├── ValueIteratorLocal.cpp ├── vi_node_after_planning.cpp ├── vi_node_no_local.cpp ├── vi_node.cpp ├── vi_node_after_planning_no_local.cpp └── ValueIterator.cpp ├── test ├── docker │ └── Dockerfile └── test.bash ├── include └── value_iteration │ ├── SweepWorkerStatus.h │ ├── StateTransition.h │ ├── State.h │ ├── Action.h │ ├── ValueIteratorLocal.h │ ├── vi_node.h │ ├── vi_node_no_local.h │ └── ValueIterator.h ├── .github └── workflows │ └── vi_test.yml ├── LICENSE ├── scripts ├── vi_controller.py └── vi_turtle_round_trip.py ├── package.xml ├── launch ├── emcl.launch ├── move_base_house.launch ├── amcl.launch ├── navigation_house_amcl.launch ├── experiments_for_robosys2022 │ ├── round_trip_experiment.launch │ ├── round_trip_experiment_no_local.launch │ ├── round_trip_experiment_offline.launch │ └── round_trip_experiment_offline_no_local.launch ├── navigation_house.launch ├── navigation_house_no_local.launch └── navigation_house_after_planning.launch ├── README.md ├── config └── conf.rviz └── CMakeLists.txt /maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ryuichiueda/value_iteration/HEAD/maps/map.pgm -------------------------------------------------------------------------------- /maps/house.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ryuichiueda/value_iteration/HEAD/maps/house.pgm -------------------------------------------------------------------------------- /maps/house_amdp.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ryuichiueda/value_iteration/HEAD/maps/house_amdp.bmp -------------------------------------------------------------------------------- /maps/house_amdp.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ryuichiueda/value_iteration/HEAD/maps/house_amdp.pgm -------------------------------------------------------------------------------- /action/Vi.action: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped goal 2 | --- 3 | bool finished 4 | --- 5 | std_msgs/UInt32MultiArray current_sweep_times 6 | std_msgs/Float32MultiArray deltas 7 | -------------------------------------------------------------------------------- /maps/house.yaml: -------------------------------------------------------------------------------- 1 | image: ./house.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: ./map.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/house_amdp.yaml: -------------------------------------------------------------------------------- 1 | image: ./house.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /maps/cost.yaml: -------------------------------------------------------------------------------- 1 | image: ./cost.pgm 2 | resolution: 0.050000 3 | origin: [-10.000000, -10.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 #not used 6 | free_thresh: 0.196 #not used 7 | mode: raw 8 | -------------------------------------------------------------------------------- /src/SweepWorkerStatus.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/ValueIterator.h" 2 | #include 3 | 4 | namespace value_iteration{ 5 | 6 | SweepWorkerStatus::SweepWorkerStatus() 7 | { 8 | _finished = false; 9 | _sweep_step = 0; 10 | _delta = ValueIterator::max_cost_; 11 | } 12 | 13 | } 14 | -------------------------------------------------------------------------------- /test/docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ryuichiueda/emcl-test-env:latest 2 | ENV DEBIAN_FRONTEND=noninteractive 3 | SHELL ["/bin/bash", "-c"] 4 | 5 | RUN cd /catkin_ws/src && \ 6 | git clone https://github.com/ryuichiueda/value_iteration.git 7 | 8 | RUN source ~/.bashrc && \ 9 | cd /catkin_ws && \ 10 | catkin_make 11 | -------------------------------------------------------------------------------- /include/value_iteration/SweepWorkerStatus.h: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_SWEEP_WS_ 2 | #define VALUE_SWEEP_WS_ 3 | 4 | namespace value_iteration{ 5 | 6 | class SweepWorkerStatus{ 7 | public: 8 | bool _finished; 9 | int _sweep_step; 10 | double _delta; 11 | 12 | SweepWorkerStatus(); 13 | }; 14 | 15 | } 16 | 17 | #endif 18 | 19 | -------------------------------------------------------------------------------- /src/Action.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/Action.h" 2 | 3 | namespace value_iteration{ 4 | 5 | Action::Action(std::string name, double fw, double rot, int id) : id_(id) 6 | { 7 | _name = name; 8 | 9 | _delta_fw = fw; 10 | _delta_rot = rot; 11 | 12 | //_delta_fw_stdev = fabs(fw)*0.1; 13 | //_delta_rot_stdev = fabs(rot)*0.1; 14 | } 15 | 16 | } 17 | -------------------------------------------------------------------------------- /include/value_iteration/StateTransition.h: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_STS_ 2 | #define VALUE_STS_ 3 | 4 | #include 5 | 6 | namespace value_iteration{ 7 | 8 | class StateTransition{ 9 | public: 10 | int _dix, _diy, _dit; 11 | int _prob; 12 | 13 | StateTransition(int dix, int diy, int dit, int prob); 14 | std::string to_string(void); 15 | }; 16 | 17 | } 18 | 19 | #endif 20 | 21 | -------------------------------------------------------------------------------- /test/test.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash -evx 2 | 3 | rm -f '/tmp/value_t=3.pgm' '/tmp/action_t=41.ppm' 4 | 5 | 6 | source ~/.bashrc 7 | source ~/catkin_ws/devel/setup.bash 8 | roslaunch value_iteration test.launch || echo Finish 9 | [ "$(md5sum < ~/catkin_ws/src/value_iteration/test/value_t=3.pgm)" = "$(md5sum < /tmp/value_t=3.pgm)" ] 10 | [ "$(md5sum < ~/catkin_ws/src/value_iteration/test/action_t=41.ppm)" = "$(md5sum < /tmp/action_t=41.ppm)" ] 11 | -------------------------------------------------------------------------------- /src/StateTransition.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/StateTransition.h" 2 | 3 | namespace value_iteration{ 4 | 5 | StateTransition::StateTransition(int dix, int diy, int dit, int prob) 6 | { 7 | _dix = dix; 8 | _diy = diy; 9 | _dit = dit; 10 | _prob = prob; 11 | } 12 | 13 | std::string StateTransition::to_string(void) 14 | { 15 | return "dix:" + std::to_string(_dix) + " diy:" + std::to_string(_diy) 16 | + " dit:" + std::to_string(_dit) + " prob:" + std::to_string(_prob); 17 | } 18 | 19 | } 20 | -------------------------------------------------------------------------------- /include/value_iteration/State.h: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_STATE_ 2 | #define VALUE_STATE_ 3 | 4 | #include "Action.h" 5 | 6 | namespace value_iteration{ 7 | 8 | using namespace std; 9 | 10 | class State{ 11 | public: 12 | uint64_t total_cost_; 13 | uint64_t penalty_; 14 | int ix_, iy_, it_; 15 | bool free_; 16 | bool final_state_; 17 | 18 | uint64_t local_penalty_; 19 | 20 | Action *optimal_action_; 21 | 22 | State(int x, int y, int theta, const nav_msgs::OccupancyGrid &map, 23 | int margin, double margin_penalty, int x_num); 24 | State(int x, int y, int theta, unsigned int cost); 25 | }; 26 | 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /include/value_iteration/Action.h: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_ACTION_ 2 | #define VALUE_ACTION_ 3 | 4 | #include "StateTransition.h" 5 | #include 6 | #include 7 | 8 | namespace value_iteration{ 9 | 10 | class Action{ 11 | public: 12 | Action(std::string name, double fw, double rot, int id); 13 | 14 | std::string _name; 15 | double _delta_fw; //forward traveling distance[m] 16 | double _delta_rot; //rotation[deg] 17 | 18 | int id_; 19 | 20 | // double _delta_fw_stdev; 21 | // double _delta_rot_stdev; 22 | 23 | std::vector< std::vector > _state_transitions; //thetaごとに状態遷移先のリストを保存 24 | }; 25 | 26 | } 27 | 28 | #endif 29 | 30 | -------------------------------------------------------------------------------- /.github/workflows/vi_test.yml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | on: 4 | push: 5 | paths-ignore: 6 | - '**.md' 7 | 8 | jobs: 9 | build: 10 | runs-on: ubuntu-18.04 11 | 12 | steps: 13 | - uses: actions/checkout@v2 14 | - name: Build 15 | run: | 16 | git clone -b "${GITHUB_REF#refs/heads/}" https://github.com/ryuichiueda/value_iteration.git 17 | cd value_iteration/test/docker 18 | sed -i "s;clone;clone -b ${GITHUB_REF#refs/heads/};" Dockerfile 19 | docker build -t test . 20 | # - name: Reset and navigation test on Gazebo 21 | # run: | 22 | # docker run test /bin/bash -c 'source ~/.bashrc && /catkin_ws/src/emcl/test/test.bash' 23 | 24 | 25 | -------------------------------------------------------------------------------- /include/value_iteration/ValueIteratorLocal.h: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_ITERATOR_LOCAL_ 2 | #define VALUE_ITERATOR_LOCAL_ 3 | 4 | #include "ValueIterator.h" 5 | 6 | namespace value_iteration{ 7 | 8 | class ValueIteratorLocal : public ValueIterator{ 9 | public: 10 | ValueIteratorLocal(vector &actions, int thread_num); 11 | 12 | uint64_t valueIterationLocal(State &s); 13 | void localValueIterationWorker(int id); 14 | 15 | void setMapWithOccupancyGrid(nav_msgs::OccupancyGrid &map, int theta_cell_num, 16 | double safety_radius, double safety_radius_penalty, 17 | double goal_margin_radius, int goal_margin_theta); 18 | 19 | Action *posToAction(double x, double y, double t_rad); 20 | 21 | void setLocalCost(const sensor_msgs::LaserScan::ConstPtr &msg, double x, double y, double t); 22 | void setLocalWindow(double x, double y); 23 | void makeLocalValueFunctionMap(nav_msgs::OccupancyGrid &map, int threshold, 24 | double x, double y, double yaw_rad); 25 | 26 | private: 27 | void localValueIterationLoop(void); 28 | 29 | bool inLocalArea(int ix, int iy); 30 | uint64_t actionCostLocal(State &s, Action &a); 31 | 32 | int local_ix_min_, local_ix_max_, local_iy_min_, local_iy_max_; 33 | int local_ixy_range_; 34 | double local_xy_range_; 35 | }; 36 | 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /src/State.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/ValueIterator.h" 2 | 3 | namespace value_iteration{ 4 | 5 | State::State(int x, int y, int theta, const nav_msgs::OccupancyGrid &map, 6 | int margin, double margin_penalty, int x_num) 7 | { 8 | if(margin_penalty > 1.0e+10) 9 | ROS_ERROR("TOO LARGE PENALTY TO VIOLATION OF SAFETY RADIUS"); 10 | 11 | ix_ = x; 12 | iy_ = y; 13 | it_ = theta; 14 | total_cost_ = ValueIterator::max_cost_; 15 | penalty_ = ValueIterator::prob_base_; 16 | local_penalty_ = 0; 17 | final_state_ = false; 18 | optimal_action_ = NULL; 19 | 20 | free_ = (map.data[y*x_num + x] == 0); 21 | if(not free_) 22 | return; 23 | 24 | for(int ix=-margin+x; ix<=margin+x; ix++){ 25 | for(int iy=-margin+y; iy<=margin+y; iy++){ 26 | int pos = iy*x_num + ix; 27 | if(0 <= pos and pos < map.data.size() and map.data[iy*x_num + ix] != 0) 28 | penalty_ = (uint64_t)(margin_penalty * ValueIterator::prob_base_) + ValueIterator::prob_base_; 29 | } 30 | } 31 | } 32 | 33 | State::State(int x, int y, int theta, unsigned int cost) 34 | { 35 | ix_ = x; 36 | iy_ = y; 37 | it_ = theta; 38 | total_cost_ = ValueIterator::max_cost_; 39 | final_state_ = false; 40 | optimal_action_ = NULL; 41 | free_ = (cost != 255); 42 | penalty_ = free_ ? (cost << ValueIterator::prob_base_bit_) : 0; 43 | } 44 | 45 | } 46 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2021, CIT autonomous robot laboratory 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright notice, 7 | this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright notice, 9 | this list of conditions and the following disclaimer in the documentation 10 | and/or other materials provided with the distribution. 11 | * Neither the name of the nor the names of its contributors 12 | may be used to endorse or promote products derived from this software 13 | without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /scripts/vi_controller.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rospy, copy 4 | from value_iteration.msg import ViAction, ViGoal 5 | from geometry_msgs.msg import PoseStamped 6 | import actionlib 7 | 8 | goal_data = None 9 | client = None 10 | 11 | def vi_state_cb(feedback): 12 | rospy.loginfo("SWEEPS: " + ", ".join(str(e) for e in feedback.current_sweep_times.data)) 13 | rospy.loginfo("DELTAS: " + ", ".join(str(e) for e in feedback.deltas.data)) 14 | 15 | def vi_client(data): 16 | global client 17 | global goal_data 18 | 19 | client = actionlib.SimpleActionClient('/vi_controller', ViAction) 20 | client.wait_for_server() 21 | 22 | goal = ViGoal() 23 | goal.goal = copy.copy(data) 24 | client.send_goal(goal, feedback_cb=vi_state_cb) 25 | client.wait_for_result() 26 | return client.get_result() 27 | 28 | def receive_goal(data): 29 | global goal_data 30 | global client 31 | 32 | if goal_data != None: 33 | client.cancel_goal() 34 | 35 | goal_data = data 36 | 37 | if __name__ == '__main__': 38 | rospy.init_node('vi_controller_turtle_env') 39 | 40 | sub_goal = rospy.Subscriber("/move_base_simple/goal", PoseStamped, receive_goal) 41 | 42 | rate = rospy.Rate(20) 43 | while not rospy.is_shutdown(): 44 | if goal_data == None: 45 | rate.sleep() 46 | continue 47 | 48 | result = vi_client(goal_data) 49 | if result.finished: 50 | goal_data = None 51 | rospy.loginfo(result) 52 | 53 | rate.sleep() 54 | 55 | rospy.spin() 56 | -------------------------------------------------------------------------------- /scripts/vi_turtle_round_trip.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rospy, math, time, tf 4 | from value_iteration.msg import ViAction, ViGoal 5 | from geometry_msgs.msg import PoseStamped 6 | import actionlib 7 | 8 | pos = [(6.0, -2.0, math.pi/2), 9 | (3.0, 3.0, math.pi/3), 10 | (4.0, -1.0, math.pi*1.5), 11 | (1.0, 2.0, 0.0), 12 | (-4.0, -1.0, 0.0), 13 | (-2.0, 4.5, math.pi), 14 | (-7.0, -3.0, math.pi/4*3), 15 | (-7.0, 2.0, math.pi/4*7) ] 16 | 17 | def vi_state_cb(feedback): 18 | rospy.loginfo("SWEEPS: " + ", ".join(str(e) for e in feedback.current_sweep_times.data)) 19 | rospy.loginfo("DELTAS: " + ", ".join(str(e) for e in feedback.deltas.data)) 20 | 21 | def vi_client(data): 22 | client = actionlib.SimpleActionClient('/vi_controller', ViAction) 23 | client.wait_for_server() 24 | 25 | goal = ViGoal() 26 | goal.goal = data 27 | client.send_goal(goal, feedback_cb=vi_state_cb) 28 | client.wait_for_result() 29 | return client.get_result() 30 | 31 | #def receive_goal(data): 32 | # result = vi_client(data) 33 | # rospy.loginfo(result) 34 | 35 | if __name__ == '__main__': 36 | time.sleep(15) 37 | rospy.init_node('vi_controller_turtle_env') 38 | 39 | for p in pos: 40 | goal = PoseStamped() 41 | goal.pose.position.x = p[0] 42 | goal.pose.position.y = p[1] 43 | q = tf.transformations.quaternion_from_euler(0.0, 0.0, p[2]) 44 | goal.pose.orientation.z = q[2] 45 | goal.pose.orientation.w = q[3] 46 | 47 | vi_client(goal) 48 | 49 | #rospy.spin() 50 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | value_iteration 4 | 0.0.1 5 | The value_iteration package 6 | 7 | 8 | Ryuichi Ueda 9 | BSD 10 | 11 | 12 | 13 | 14 | roscpp 15 | rospy 16 | grid_map_core 17 | grid_map_ros 18 | grid_map_cv 19 | grid_map_filters 20 | grid_map_loader 21 | grid_map_msgs 22 | grid_map_octomap 23 | grid_map_rviz_plugin 24 | grid_map_visualization 25 | filters 26 | 27 | message_generation 28 | geometry_msgs 29 | actionlib 30 | actionlib_msgs 31 | 32 | message_generation 33 | 34 | message_runtime 35 | geometry_msgs 36 | actionlib 37 | actionlib_msgs 38 | 39 | catkin 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /launch/emcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /include/value_iteration/vi_node.h: -------------------------------------------------------------------------------- 1 | #ifndef _VI_NODE_H__ 2 | #define _VI_NODE_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "geometry_msgs/Twist.h" 9 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 10 | 11 | #include "nav_msgs/GetMap.h" 12 | #include "nav_msgs/OccupancyGrid.h" 13 | #include "value_iteration/ValueIteratorLocal.h" 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | namespace value_iteration{ 25 | 26 | class ViNode{ 27 | 28 | public: 29 | ViNode(); 30 | ~ViNode(); 31 | 32 | void pubValueFunction(void); 33 | void decision(void); 34 | private: 35 | vector *actions_; 36 | shared_ptr vi_; 37 | ros::NodeHandle nh_; 38 | ros::NodeHandle private_nh_; 39 | 40 | ros::ServiceServer srv_policy_; 41 | ros::ServiceServer srv_value_; 42 | 43 | ros::Publisher pub_cmd_vel_; 44 | ros::Publisher pub_value_function_; 45 | ros::Subscriber sub_laser_scan_; 46 | 47 | tf::TransformListener tf_listener_; 48 | 49 | shared_ptr > as_; 50 | 51 | void executeVi(const value_iteration::ViGoalConstPtr &goal); 52 | bool servePolicy(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); 53 | bool serveValue(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); 54 | 55 | void scanReceived(const sensor_msgs::LaserScan::ConstPtr &msg); 56 | 57 | void setActions(void); 58 | void setMap(nav_msgs::GetMap::Response &res); 59 | void setCommunication(void); 60 | 61 | double x_, y_, yaw_; 62 | 63 | //string status_; 64 | bool online_; 65 | 66 | int cost_drawing_threshold_; 67 | }; 68 | 69 | } 70 | #endif 71 | -------------------------------------------------------------------------------- /include/value_iteration/vi_node_no_local.h: -------------------------------------------------------------------------------- 1 | #ifndef _VI_NODE_NL_H__ 2 | #define _VI_NODE_NL_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "geometry_msgs/Twist.h" 9 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 10 | 11 | #include "nav_msgs/GetMap.h" 12 | #include "nav_msgs/OccupancyGrid.h" 13 | #include "value_iteration/ValueIterator.h" 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | namespace value_iteration{ 25 | 26 | class ViNode{ 27 | 28 | public: 29 | ViNode(); 30 | ~ViNode(); 31 | 32 | void pubValueFunction(void); 33 | void decision(void); 34 | private: 35 | vector *actions_; 36 | shared_ptr vi_; 37 | ros::NodeHandle nh_; 38 | ros::NodeHandle private_nh_; 39 | 40 | ros::ServiceServer srv_policy_; 41 | ros::ServiceServer srv_value_; 42 | 43 | ros::Publisher pub_cmd_vel_; 44 | ros::Publisher pub_value_function_; 45 | ros::Publisher pub_local_value_function_; 46 | //ros::Subscriber sub_pose_; 47 | ros::Subscriber sub_laser_scan_; 48 | 49 | tf::TransformListener tf_listener_; 50 | 51 | shared_ptr > as_; 52 | 53 | void executeVi(const value_iteration::ViGoalConstPtr &goal); 54 | bool servePolicy(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); 55 | bool serveValue(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); 56 | 57 | void scanReceived(const sensor_msgs::LaserScan::ConstPtr &msg); 58 | 59 | void setActions(void); 60 | void setMap(nav_msgs::GetMap::Response &res); 61 | void setCommunication(void); 62 | 63 | double x_, y_, yaw_; 64 | 65 | //string status_; 66 | bool online_; 67 | 68 | int cost_drawing_threshold_; 69 | }; 70 | 71 | } 72 | #endif 73 | -------------------------------------------------------------------------------- /launch/move_base_house.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /launch/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /launch/navigation_house_amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /launch/experiments_for_robosys2022/round_trip_experiment.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /launch/experiments_for_robosys2022/round_trip_experiment_no_local.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /launch/experiments_for_robosys2022/round_trip_experiment_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /launch/navigation_house.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /launch/experiments_for_robosys2022/round_trip_experiment_offline_no_local.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /launch/navigation_house_no_local.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /launch/navigation_house_after_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | vi_node: 12 | action_list: 13 | - name: forward 14 | onestep_forward_m: 0.3 15 | onestep_rotation_deg: 0.0 16 | - name: back 17 | onestep_forward_m: -0.2 18 | onestep_rotation_deg: 0.0 19 | - name: right 20 | onestep_forward_m: 0.0 21 | onestep_rotation_deg: -20.0 22 | - name: rightfw 23 | onestep_forward_m: 0.2 24 | onestep_rotation_deg: -20.0 25 | - name: left 26 | onestep_forward_m: 0.0 27 | onestep_rotation_deg: 20.0 28 | - name: leftfw 29 | onestep_forward_m: 0.2 30 | onestep_rotation_deg: 20.0 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /include/value_iteration/ValueIterator.h: -------------------------------------------------------------------------------- 1 | #ifndef VALUE_ITERATOR_ 2 | #define VALUE_ITERATOR_ 3 | 4 | #include "ros/ros.h" 5 | #include "std_msgs/UInt32MultiArray.h" 6 | #include "std_msgs/Float32MultiArray.h" 7 | #include "nav_msgs/GetMap.h" 8 | #include "nav_msgs/OccupancyGrid.h" 9 | #include "sensor_msgs/LaserScan.h" 10 | #include 11 | #include 12 | #include 13 | 14 | #include "SweepWorkerStatus.h" 15 | #include "Action.h" 16 | #include "State.h" 17 | 18 | namespace value_iteration{ 19 | 20 | class ValueIterator{ 21 | protected: 22 | /* value iteration */ 23 | std::vector states_; 24 | std::vector &actions_; 25 | std::vector > sweep_orders_; 26 | 27 | uint64_t valueIteration(State &s); 28 | uint64_t actionCost(State &s, Action &a); 29 | public: 30 | void setGoal(double goal_x, double goal_y, int goal_t); 31 | void valueIterationWorker(int times, int id); 32 | 33 | /* calculation */ 34 | protected: 35 | int toIndex(int ix, int iy, int it); 36 | bool inMapArea(int ix, int iy); 37 | void cellDelta(double x, double y, double t, int &ix, int &iy, int &it); 38 | void noNoiseStateTransition(Action &a, double from_x, double from_y, double from_t, double &to_x, double &to_y, double &to_t); 39 | 40 | /* robot control */ 41 | public: 42 | bool endOfTrial(void); 43 | bool arrived(void); 44 | Action *posToAction(double x, double y, double t_rad); 45 | void setCalculated(void); 46 | bool isCalculated(void); 47 | 48 | /* initialization */ 49 | ValueIterator(vector &actions, int thread_num); 50 | void setMapWithOccupancyGrid(nav_msgs::OccupancyGrid &map, int theta_cell_num, 51 | double safety_radius, double safety_radius_penalty, 52 | double goal_margin_radius, int goal_margin_theta); 53 | 54 | void setMapWithCostGrid(nav_msgs::OccupancyGrid &map, int theta_cell_num, 55 | double safety_radius, double safety_radius_penalty, 56 | double goal_margin_radius, int goal_margin_theta); 57 | protected: 58 | void setState(const nav_msgs::OccupancyGrid &map, double safety_radius, double safety_radius_penalty); 59 | void setStateValues(void); 60 | void setStateTransition(void); 61 | void setStateTransitionWorker(int it); 62 | void setStateTransitionWorkerSub(Action &a, int it); 63 | void setSweepOrders(void); 64 | 65 | /* ros output */ 66 | public: 67 | bool policyWriter(grid_map_msgs::GetGridMap::Response& response); 68 | bool valueFunctionWriter(grid_map_msgs::GetGridMap::Response& response); 69 | void makeValueFunctionMap(nav_msgs::OccupancyGrid &map, int threshold, 70 | double x, double y, double yaw_rad); 71 | 72 | /* control of value iteration threads */ 73 | map thread_status_; 74 | bool finished(std_msgs::UInt32MultiArray &sweep_times, std_msgs::Float32MultiArray &deltas); 75 | void setCancel(void); 76 | protected: 77 | string status_; 78 | 79 | /* parameters */ 80 | public: 81 | double goal_x_, goal_y_, goal_margin_radius_; 82 | int goal_t_, goal_margin_theta_; 83 | int thread_num_; 84 | const static uint64_t max_cost_; 85 | const static uint64_t prob_base_; 86 | const static unsigned char prob_base_bit_; 87 | protected: 88 | double xy_resolution_, t_resolution_; 89 | int cell_num_x_, cell_num_y_, cell_num_t_; 90 | double map_origin_x_; 91 | double map_origin_y_; 92 | geometry_msgs::Quaternion map_origin_quat_; 93 | const static unsigned char resolution_xy_bit_, resolution_t_bit_; 94 | 95 | /* for local value iteration */ 96 | }; 97 | 98 | const unsigned char ValueIterator::resolution_xy_bit_ = 6; 99 | const unsigned char ValueIterator::resolution_t_bit_ = 6; 100 | const unsigned char ValueIterator::prob_base_bit_ = ValueIterator::resolution_xy_bit_*2+ValueIterator::resolution_t_bit_; 101 | const uint64_t ValueIterator::prob_base_ = 1< &actions, int thread_num) : ValueIterator(actions, thread_num) 6 | { 7 | local_ix_min_ = local_ix_max_ = local_iy_min_ = local_iy_max_ = 0; 8 | } 9 | 10 | void ValueIteratorLocal::setMapWithOccupancyGrid(nav_msgs::OccupancyGrid &map, int theta_cell_num, 11 | double safety_radius, double safety_radius_penalty, 12 | double goal_margin_radius, int goal_margin_theta) 13 | { 14 | ValueIterator::setMapWithOccupancyGrid(map, theta_cell_num, safety_radius, safety_radius_penalty, 15 | goal_margin_radius, goal_margin_theta); 16 | 17 | local_xy_range_ = 1.0; 18 | local_ixy_range_ = (int)(local_xy_range_/xy_resolution_); 19 | local_ix_min_ = 0; 20 | local_iy_min_ = 0; 21 | local_ix_max_ = local_ixy_range_*2; 22 | local_iy_max_ = local_ixy_range_*2; 23 | } 24 | 25 | void ValueIteratorLocal::localValueIterationWorker(int id) 26 | { 27 | while(status_ == "canceled" or status_ == "goal"){ 28 | ROS_INFO("STATUS PROBLEM: %s", status_.c_str()); 29 | status_ = "executing"; 30 | } 31 | 32 | while(status_ != "canceled" and status_ != "goal"){ 33 | localValueIterationLoop(); 34 | } 35 | } 36 | 37 | void ValueIteratorLocal::localValueIterationLoop(void) 38 | { 39 | for(int iix=local_ix_min_;iix<=local_ix_max_;iix++){ 40 | for(int iiy=local_iy_min_;iiy<=local_iy_max_;iiy++){ 41 | for(int iit=0;iit 0 ? delta : -delta; 69 | } 70 | 71 | Action *ValueIteratorLocal::posToAction(double x, double y, double t_rad) 72 | { 73 | int ix = (int)floor( (x - map_origin_x_)/xy_resolution_ ); 74 | int iy = (int)floor( (y - map_origin_y_)/xy_resolution_ ); 75 | 76 | int t = (int)(180 * t_rad / M_PI); 77 | int it = (int)floor( ( (t + 360*100)%360 )/t_resolution_ ); 78 | int index = toIndex(ix, iy, it); 79 | 80 | if(states_[index].final_state_){ 81 | status_ = "goal"; 82 | return NULL; 83 | /* 84 | }else if(states_[index].local_optimal_action_ != NULL){ 85 | ROS_INFO("COST TO GO: %f", (double)states_[index].local_total_cost_/ValueIterator::prob_base_); 86 | return states_[index].local_optimal_action_; 87 | */ 88 | }else if(states_[index].optimal_action_ != NULL){ 89 | ROS_INFO("COST TO GO: %f", (double)states_[index].total_cost_/ValueIterator::prob_base_); 90 | return states_[index].optimal_action_; 91 | } 92 | 93 | return NULL; 94 | 95 | } 96 | 97 | bool ValueIteratorLocal::inLocalArea(int ix, int iy) 98 | { 99 | return ix >= local_ix_min_ and ix <= local_ix_max_ and iy >= local_iy_min_ and iy <= local_iy_max_; 100 | } 101 | 102 | void ValueIteratorLocal::setLocalCost(const sensor_msgs::LaserScan::ConstPtr &msg, double x, double y, double t) 103 | { 104 | double start_angle = msg->angle_min; 105 | for(int i=0; iranges.size(); i++){ 106 | double a = t + msg->angle_increment*i + start_angle; 107 | 108 | double lx = x + msg->ranges[i]*cos(a); 109 | double ly = y + msg->ranges[i]*sin(a); 110 | int ix = (int)floor( (lx - map_origin_x_)/xy_resolution_ ); 111 | int iy = (int)floor( (ly - map_origin_y_)/xy_resolution_ ); 112 | 113 | for(double d=0.1;d<=0.9;d+=0.1){ 114 | double half_lx = x + msg->ranges[i]*cos(a)*d; 115 | double half_ly = y + msg->ranges[i]*sin(a)*d; 116 | int half_ix = (int)floor( (half_lx - map_origin_x_)/xy_resolution_ ); 117 | int half_iy = (int)floor( (half_ly - map_origin_y_)/xy_resolution_ ); 118 | 119 | if(not inLocalArea(half_ix, half_iy)) 120 | continue; 121 | 122 | for(int it=0;it= cell_num_x_) 150 | return max_cost_; 151 | 152 | int iy = s.iy_ + tran._diy; 153 | if(iy < 0 or iy >= cell_num_y_) 154 | return max_cost_; 155 | 156 | int it = (tran._dit + cell_num_t_)%cell_num_t_; 157 | 158 | auto &after_s = states_[toIndex(ix, iy, it)]; 159 | if(not after_s.free_) 160 | return max_cost_; 161 | 162 | cost += ( after_s.total_cost_ + after_s.penalty_ + after_s.local_penalty_ ) * tran._prob; 163 | } 164 | 165 | return cost >> prob_base_bit_; 166 | } 167 | 168 | void ValueIteratorLocal::setLocalWindow(double x, double y) 169 | { 170 | int ix = (int)floor( (x - map_origin_x_)/xy_resolution_ ); 171 | int iy = (int)floor( (y - map_origin_y_)/xy_resolution_ ); 172 | 173 | local_ix_min_ = ix - local_ixy_range_ >=0 ? ix - local_ixy_range_ : 0; 174 | local_iy_min_ = iy - local_ixy_range_ >=0 ? iy - local_ixy_range_ : 0; 175 | local_ix_max_ = ix + local_ixy_range_ < cell_num_x_ ? ix + local_ixy_range_ : cell_num_x_-1; 176 | local_iy_max_ = iy + local_ixy_range_ < cell_num_y_ ? iy + local_ixy_range_ : cell_num_y_-1; 177 | } 178 | 179 | void ValueIteratorLocal::makeLocalValueFunctionMap(nav_msgs::OccupancyGrid &map, int threshold, 180 | double x, double y, double yaw_rad) 181 | { 182 | map.header.stamp = ros::Time::now(); 183 | map.header.frame_id = "map"; 184 | map.info.resolution = xy_resolution_; 185 | map.info.width = local_ixy_range_*2 + 1; 186 | map.info.height = local_ixy_range_*2 + 1; 187 | map.info.origin.position.x = x - local_xy_range_; 188 | map.info.origin.position.y = y - local_xy_range_; 189 | map.info.origin.orientation = map_origin_quat_; 190 | 191 | int it = (int)floor( ( ((int)(yaw_rad/M_PI*180) + 360*100)%360 )/t_resolution_ ); 192 | 193 | for(int y=local_iy_min_; y<=local_iy_max_; y++) 194 | for(int x=local_ix_min_; x<=local_ix_max_; x++){ 195 | int index = toIndex(x, y, it); 196 | double cost = (double)states_[index].total_cost_/prob_base_; 197 | if(cost < (double)threshold) 198 | map.data.push_back((int)(cost/threshold*250)); 199 | else if(states_[index].free_) 200 | map.data.push_back(250); 201 | else 202 | map.data.push_back(255); 203 | } 204 | } 205 | 206 | } 207 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # value_iteration: real-time value iteration planner package for ROS 2 | 3 | a monolithic mobile robot navigation system as a replacement for the navigation stack 4 | 5 | ## How to try 6 | 7 | Please give the navigation goal and some obstacles as shown in the movie below after the command: 8 | 9 | ``` 10 | $ roslaunch value_iteration navigation_house_amcl.launch 11 | ``` 12 | 13 | [![](https://img.youtube.com/vi/O1NJnF0sIdA/0.jpg)](https://www.youtube.com/watch?v=O1NJnF0sIdA) 14 | 15 | ## Nodes 16 | 17 | ### vi_node 18 | 19 | This node executes value iteration. 20 | 21 | #### how to execute value iteration 22 | 23 | The value iteration procedure rises up through the action `value_iteration/ViAction`, which is composed of the following goal, feedback, and result. An example of usage is shown in [vi_turtle_env.py](https://github.com/ryuichiueda/value_iteration/blob/main/scripts/vi_turtle_env.py). 24 | 25 | * /vi_controller/goal 26 | * goal ([geometry_msgs/PoseStamped](http://docs.ros.org/en/melodic/api/geometry_msgs/html/msg/PoseStamped.html)) 27 | * the destination 28 | * /vi_controller/feedback 29 | * current_sweep_times (std_msgs/UInt32MultiArray) 30 | * the number of sweeps executed so far by each thread 31 | * deltas (std_msgs/Float32MultiArray) 32 | * the maximum change of state values caused by the latest sweep of each thread 33 | * /vi_controller/result 34 | * finished (bool) 35 | * return true after the completion of value iteration 36 | 37 | #### Services 38 | 39 | * policy ([grid_map_msgs/GetGridMap](http://docs.ros.org/en/kinetic/api/grid_map_msgs/html/srv/GetGridMap.html)) 40 | * Provide calculated policy. The id of the optimal action is written as a float value in each cell. 41 | * value ([grid_map_msgs/GetGridMap](http://docs.ros.org/en/kinetic/api/grid_map_msgs/html/srv/GetGridMap.html)) 42 | * Provide calculated value function. 43 | * action_list (not implemented) 44 | * Provide the id and velocity of every action. 45 | 46 | #### Services Called 47 | 48 | * static_map ([nav_msgs/GetMap](http://docs.ros.org/en/api/nav_msgs/html/srv/GetMap.html)) 49 | * Initiate the map for value iteration. 50 | * cost_map ([nav_msgs/GetMap](http://docs.ros.org/en/api/nav_msgs/html/srv/GetMap.html)) 51 | * It can be used instead of `static_map`. (please read Maps section) 52 | 53 | #### Subscribed Topics 54 | 55 | * scan ([sensor_msgs/LaserScan](http://docs.ros.org/en/api/sensor_msgs/html/msg/LaserScan.html)) 56 | * laser scans 57 | * tf ([tf/tfMessage](http://docs.ros.org/en/api/tf/html/msg/tfMessage.html)) 58 | * transforms 59 | 60 | #### Published Topics 61 | 62 | * /cmd_vel ([geometry_msgs/Twist](http://docs.ros.org/en/melodic/api/geometry_msgs/html/msg/Twist.html)) 63 | * the control order to the robot; published only when the parameter `online` is true 64 | 65 | 66 | #### Parameters 67 | 68 | * action_list 69 | * Currently, `vi_node` requires an action list like the following. `onestep_forward_m` and `onestep_rotation_deg` mean the forward and rotational velocities respectively. `vi_node` calculates the optimal value function and the oplimal policy based on this action list. 70 | 71 | ``` 72 | 73 | vi_node: 74 | action_list: 75 | - name: forward 76 | onestep_forward_m: 0.3 77 | onestep_rotation_deg: 0.0 78 | - name: back 79 | onestep_forward_m: -0.2 80 | onestep_rotation_deg: 0.0 81 | - name: right 82 | onestep_forward_m: 0.0 83 | onestep_rotation_deg: -20.0 84 | - name: left 85 | onestep_forward_m: 0.0 86 | onestep_rotation_deg: 20.0 87 | 88 | ``` 89 | 90 | * ~online (bool, defalut: false) 91 | * flag for using vi_node as a real-time planner 92 | * ~theta_cell_num (int, default: 60) 93 | * number of intervals of the discrete state space on theta-axis 94 | * ~thread_num (int, default: 4) 95 | * number of threads used on value iteration 96 | * ~goal_margin_radius (double, default: 0.2[m]) 97 | * radius of the goal on xy-plane 98 | * ~goal_margin_theta (int, default: 10[deg]) 99 | * radius of the goal on theta-axis 100 | * ~map_type (string, "cost" or "occupancy", default: occupancy) 101 | * choice of map for setting immediate costs and occupancy (please read the Maps section) 102 | * ~cost_drawing_threshold (int, default: 60[s]) 103 | * cost value related to the maximum gradation color 104 | 105 | #### Maps 106 | 107 | We can choose two types of maps for initializing parametes of states. 108 | 109 | ##### occupancy mode: 110 | 111 | A cost map is created from an occupancy grid map. Occupied cells are marked as roped-off area. Moreover, cells near occupied ones are given immediate penalty. The near cells and the value of the penalty are controlled through the following parametes. 112 | 113 | * parapeters 114 | * ~safety_radius (double, default: 0.2[m]) 115 | * distance that should be kept between the center of the robot and an occupancy grid 116 | * ~safety_radius_penalty (double, default: 30[s], max: 1,000,000,000[s]) 117 | * immediate penality (negative immediate reward in the field of reinforcement learning) when the robot invades the safety radius. 118 | 119 | ##### cost mode: 120 | 121 | We can also use a "cost map," which contains the immediate cost of every cell. This map should be written with "[Raw mode](http://wiki.ros.org/map_server#Raw)". In a map, cells given 255 are regarded as roped-off cells. Cells with other values are free cells but are given the values as their immediate costs. 122 | 123 | There are an information file (`cost.yaml`) and a map file (`cost.pgm`) in the `maps` directory. The information file must contains the line `mode: raw` as shown below. 124 | 125 | ``` 126 | image: ./cost.pgm 127 | resolution: 0.050000 128 | origin: [-10.000000, -10.000000, 0.000000] 129 | negate: 0 130 | occupied_thresh: 0.65 #not used 131 | free_thresh: 0.196 #not used 132 | mode: raw 133 | ``` 134 | 135 | `vi_node` reads a cost map from a service `cost_map`. In `launch/vi_turtle_online.launch`, you can find the following lines so as to provice the cost map through `cost_map`. 136 | 137 | ``` 138 | 139 | 140 | 141 | 142 | ``` 143 | 144 | ### vi_controller_turtle_env 145 | 146 | This node receives the 2D Nav Goal from RViz and sends it to vi_node. It is implemented in `scripts/vi_turtle_env.py`. 147 | 148 | ## Notes 149 | 150 | ### What's the cost means? 151 | 152 | It is the sum of costs from a pose (position + orientation) to a point of goal. The cost means the time. Therefore, it means the time required to reach the goal if immediate penarties don't exist. As shown in the parameter list, we can define some kinds of immediate penalties, which are quantified in units of seconds. 153 | 154 | ## acknowledgement 155 | 156 | - This software is developped on the support of JSPS KAKENHI JP20K04382. 157 | - A custom simulation environment in this repository is derived from [ROBOTIS-GIT/turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations), which is licensed with Apache License 2.0. 158 | 159 | -------------------------------------------------------------------------------- /src/vi_node_after_planning.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/vi_node.h" 2 | 3 | namespace value_iteration{ 4 | 5 | ViNode::ViNode() : private_nh_("~"), yaw_(0.0), x_(0.0), y_(0.0), online_("false") 6 | { 7 | setActions(); 8 | 9 | int thread_num; 10 | private_nh_.param("thread_num", thread_num, 1); 11 | vi_.reset(new ValueIteratorLocal(*actions_, thread_num)); 12 | 13 | private_nh_.param("cost_drawing_threshold", cost_drawing_threshold_, 60); 14 | 15 | setCommunication(); 16 | 17 | nav_msgs::GetMap::Response res; 18 | setMap(res); 19 | } 20 | 21 | ViNode::~ViNode() 22 | { 23 | delete actions_; 24 | } 25 | 26 | void ViNode::setMap(nav_msgs::GetMap::Response &res) 27 | { 28 | int theta_cell_num; 29 | double safety_radius; 30 | double safety_radius_penalty; 31 | double goal_margin_radius; 32 | int goal_margin_theta; 33 | 34 | private_nh_.param("theta_cell_num", theta_cell_num, 60); 35 | private_nh_.param("safety_radius", safety_radius, 0.2); 36 | private_nh_.param("safety_radius_penalty", safety_radius_penalty, 30.0); 37 | private_nh_.param("goal_margin_radius", goal_margin_radius, 0.2); 38 | private_nh_.param("goal_margin_theta", goal_margin_theta, 10); 39 | 40 | std::string map_type; 41 | private_nh_.param("map_type", map_type, std::string("occupancy")); 42 | 43 | if(map_type == "occupancy"){ 44 | while(!ros::service::waitForService("/static_map", ros::Duration(3.0))){ 45 | ROS_INFO("Waiting for static_map"); 46 | } 47 | 48 | ros::ServiceClient client = nh_.serviceClient("/static_map"); 49 | nav_msgs::GetMap::Request req; 50 | if(not client.call(req, res)){ 51 | ROS_ERROR("static_map not working"); 52 | exit(1); 53 | } 54 | 55 | vi_->setMapWithOccupancyGrid(res.map, theta_cell_num, safety_radius, safety_radius_penalty, 56 | goal_margin_radius, goal_margin_theta); 57 | }else if(map_type == "cost"){ 58 | while(!ros::service::waitForService("/cost_map", ros::Duration(3.0))){ 59 | ROS_INFO("Waiting for cost_map"); 60 | } 61 | 62 | ros::ServiceClient client = nh_.serviceClient("/cost_map"); 63 | nav_msgs::GetMap::Request req; 64 | if(not client.call(req, res)){ 65 | ROS_ERROR("cost_map not working"); 66 | exit(1); 67 | } 68 | for(int i=0;i<100;i++) 69 | ROS_INFO("%u", (unsigned int)(res.map.data[i] & 0xFF)); 70 | 71 | vi_->setMapWithCostGrid(res.map, theta_cell_num, safety_radius, safety_radius_penalty, 72 | goal_margin_radius, goal_margin_theta); 73 | }else{ 74 | ROS_ERROR("NO SUPPORT MAP TYPE"); 75 | exit(1); 76 | } 77 | } 78 | 79 | void ViNode::setCommunication(void) 80 | { 81 | private_nh_.param("online", online_, false); 82 | if(online_){ 83 | ROS_INFO("SET ONLINE"); 84 | pub_cmd_vel_ = nh_.advertise("cmd_vel", 2, true); 85 | sub_laser_scan_ = nh_.subscribe("scan", 2, &ViNode::scanReceived, this); 86 | } 87 | 88 | pub_value_function_ = nh_.advertise("value_function", 2, true); 89 | 90 | as_.reset(new actionlib::SimpleActionServer( nh_, "vi_controller", 91 | boost::bind(&ViNode::executeVi, this, _1), false)); 92 | as_->start(); 93 | srv_policy_ = nh_.advertiseService("/policy", &ViNode::servePolicy, this); 94 | srv_value_ = nh_.advertiseService("/value", &ViNode::serveValue, this); 95 | } 96 | 97 | void ViNode::setActions(void) 98 | { 99 | XmlRpc::XmlRpcValue params; 100 | nh_.getParam("/vi_node", params); 101 | ROS_ASSERT(params.getType() == XmlRpc::XmlRpcValue::TypeStruct); 102 | auto &action_list = params["action_list"]; 103 | ROS_ASSERT(action_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 104 | actions_ = new vector(); 105 | 106 | for(int i=0; ipush_back(Action(a["name"], a["onestep_forward_m"], a["onestep_rotation_deg"], i)); 109 | 110 | auto &b = actions_->back(); 111 | ROS_INFO("set an action: %s, %f, %f", b._name.c_str(), b._delta_fw, b._delta_rot); 112 | } 113 | } 114 | 115 | void ViNode::scanReceived(const sensor_msgs::LaserScan::ConstPtr &msg) 116 | { 117 | vi_->setLocalCost(msg, x_, y_, yaw_); 118 | } 119 | 120 | bool ViNode::servePolicy(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) 121 | { 122 | vi_->policyWriter(response); 123 | return true; 124 | } 125 | 126 | bool ViNode::serveValue(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) 127 | { 128 | vi_->valueFunctionWriter(response); 129 | return true; 130 | } 131 | 132 | void ViNode::executeVi(const value_iteration::ViGoalConstPtr &goal) 133 | { 134 | ROS_INFO("VALUE ITERATION START"); 135 | auto &ori = goal->goal.pose.orientation; 136 | tf::Quaternion q(ori.x, ori.y, ori.z, ori.w); 137 | double roll, pitch, yaw; 138 | tf::Matrix3x3(q).getRPY(roll, pitch, yaw); 139 | int t = (int)(yaw*180/M_PI); 140 | vi_->setGoal(goal->goal.pose.position.x, goal->goal.pose.position.y, t); 141 | 142 | vector ths; 143 | for(int t=0; tthread_num_; t++) 144 | ths.push_back(thread(&ValueIterator::valueIterationWorker, vi_.get(), INT_MAX, t)); 145 | 146 | value_iteration::ViFeedback vi_feedback; 147 | 148 | ros::Rate loop_rate(10); 149 | while(not vi_->finished(vi_feedback.current_sweep_times, vi_feedback.deltas)){ 150 | as_->publishFeedback(vi_feedback); 151 | 152 | if(as_->isPreemptRequested()) 153 | vi_->setCancel(); 154 | 155 | loop_rate.sleep(); 156 | } 157 | as_->publishFeedback(vi_feedback); 158 | 159 | for(auto &th : ths) 160 | th.join(); 161 | 162 | vi_->setCalculated(); 163 | 164 | ROS_INFO("VALUE ITERATION END"); 165 | for(int t=0;t<4;t++) 166 | thread(&ValueIteratorLocal::localValueIterationWorker, vi_.get(), t).detach(); 167 | 168 | while(online_ and not vi_->endOfTrial() ) 169 | if(as_->isPreemptRequested()){ 170 | vi_->setCancel(); 171 | 172 | loop_rate.sleep(); 173 | } 174 | 175 | ROS_INFO("END OF TRIAL"); 176 | value_iteration::ViResult vi_result; 177 | vi_result.finished = vi_->arrived() or (not online_); 178 | as_->setSucceeded(vi_result); 179 | } 180 | 181 | 182 | void ViNode::pubValueFunction(void) 183 | { 184 | nav_msgs::OccupancyGrid map, local_map; 185 | 186 | vi_->makeValueFunctionMap(map, cost_drawing_threshold_, x_, y_, yaw_); 187 | pub_value_function_.publish(map); 188 | } 189 | 190 | void ViNode::decision(void) 191 | { 192 | if(not vi_->isCalculated()) 193 | return; 194 | 195 | try{ 196 | tf::StampedTransform trans; 197 | tf_listener_.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(0.1)); 198 | tf_listener_.lookupTransform("map", "base_link", ros::Time(0), trans); 199 | x_ = trans.getOrigin().x(); 200 | y_ = trans.getOrigin().y(); 201 | yaw_ = tf::getYaw(trans.getRotation()); 202 | }catch(tf::TransformException &e){ 203 | ROS_WARN("%s", e.what()); 204 | } 205 | 206 | vi_->setLocalWindow(x_, y_); 207 | 208 | geometry_msgs::Twist cmd_vel; 209 | cmd_vel.linear.x = 0.0; 210 | cmd_vel.angular.z = 0.0; 211 | 212 | Action *a = vi_->posToAction(x_, y_, yaw_); 213 | if(a != NULL){ 214 | cmd_vel.linear.x = a->_delta_fw; 215 | cmd_vel.angular.z = a->_delta_rot/180*M_PI; 216 | } 217 | pub_cmd_vel_.publish(cmd_vel); 218 | } 219 | 220 | } 221 | 222 | int main(int argc, char **argv) 223 | { 224 | ros::init(argc,argv,"vi_node"); 225 | value_iteration::ViNode vi_node; 226 | 227 | int step = 0; 228 | ros::Rate loop_rate(10); 229 | while(ros::ok()){ 230 | vi_node.decision(); 231 | 232 | if(step % 30 == 0) 233 | vi_node.pubValueFunction(); 234 | 235 | ros::spinOnce(); 236 | loop_rate.sleep(); 237 | step++; 238 | } 239 | 240 | return 0; 241 | } 242 | -------------------------------------------------------------------------------- /src/vi_node_no_local.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/vi_node_no_local.h" 2 | 3 | namespace value_iteration{ 4 | 5 | ViNode::ViNode() : private_nh_("~"), yaw_(0.0), x_(0.0), y_(0.0), online_("false") 6 | { 7 | setActions(); 8 | 9 | int thread_num; 10 | private_nh_.param("thread_num", thread_num, 1); 11 | vi_.reset(new ValueIterator(*actions_, thread_num)); 12 | 13 | private_nh_.param("cost_drawing_threshold", cost_drawing_threshold_, 60); 14 | 15 | setCommunication(); 16 | 17 | nav_msgs::GetMap::Response res; 18 | setMap(res); 19 | } 20 | 21 | ViNode::~ViNode() 22 | { 23 | delete actions_; 24 | } 25 | 26 | void ViNode::setMap(nav_msgs::GetMap::Response &res) 27 | { 28 | int theta_cell_num; 29 | double safety_radius; 30 | double safety_radius_penalty; 31 | double goal_margin_radius; 32 | int goal_margin_theta; 33 | 34 | private_nh_.param("theta_cell_num", theta_cell_num, 60); 35 | private_nh_.param("safety_radius", safety_radius, 0.2); 36 | private_nh_.param("safety_radius_penalty", safety_radius_penalty, 30.0); 37 | private_nh_.param("goal_margin_radius", goal_margin_radius, 0.2); 38 | private_nh_.param("goal_margin_theta", goal_margin_theta, 10); 39 | 40 | std::string map_type; 41 | private_nh_.param("map_type", map_type, std::string("occupancy")); 42 | 43 | if(map_type == "occupancy"){ 44 | while(!ros::service::waitForService("/static_map", ros::Duration(3.0))){ 45 | ROS_INFO("Waiting for static_map"); 46 | } 47 | 48 | ros::ServiceClient client = nh_.serviceClient("/static_map"); 49 | nav_msgs::GetMap::Request req; 50 | if(not client.call(req, res)){ 51 | ROS_ERROR("static_map not working"); 52 | exit(1); 53 | } 54 | 55 | vi_->setMapWithOccupancyGrid(res.map, theta_cell_num, safety_radius, safety_radius_penalty, 56 | goal_margin_radius, goal_margin_theta); 57 | }else if(map_type == "cost"){ 58 | while(!ros::service::waitForService("/cost_map", ros::Duration(3.0))){ 59 | ROS_INFO("Waiting for cost_map"); 60 | } 61 | 62 | ros::ServiceClient client = nh_.serviceClient("/cost_map"); 63 | nav_msgs::GetMap::Request req; 64 | if(not client.call(req, res)){ 65 | ROS_ERROR("cost_map not working"); 66 | exit(1); 67 | } 68 | for(int i=0;i<100;i++) 69 | ROS_INFO("%u", (unsigned int)(res.map.data[i] & 0xFF)); 70 | 71 | vi_->setMapWithCostGrid(res.map, theta_cell_num, safety_radius, safety_radius_penalty, 72 | goal_margin_radius, goal_margin_theta); 73 | }else{ 74 | ROS_ERROR("NO SUPPORT MAP TYPE"); 75 | exit(1); 76 | } 77 | } 78 | 79 | void ViNode::setCommunication(void) 80 | { 81 | private_nh_.param("online", online_, false); 82 | if(online_){ 83 | ROS_INFO("SET ONLINE"); 84 | pub_cmd_vel_ = nh_.advertise("cmd_vel", 2, true); 85 | //sub_pose_ = nh_.subscribe("mcl_pose", 2, &ViNode::poseReceived, this); 86 | sub_laser_scan_ = nh_.subscribe("scan", 2, &ViNode::scanReceived, this); 87 | } 88 | 89 | pub_value_function_ = nh_.advertise("value_function", 2, true); 90 | pub_local_value_function_ = nh_.advertise("local_value_function", 2, true); 91 | 92 | as_.reset(new actionlib::SimpleActionServer( nh_, "vi_controller", 93 | boost::bind(&ViNode::executeVi, this, _1), false)); 94 | as_->start(); 95 | srv_policy_ = nh_.advertiseService("/policy", &ViNode::servePolicy, this); 96 | srv_value_ = nh_.advertiseService("/value", &ViNode::serveValue, this); 97 | } 98 | 99 | void ViNode::setActions(void) 100 | { 101 | XmlRpc::XmlRpcValue params; 102 | nh_.getParam("/vi_node", params); 103 | ROS_ASSERT(params.getType() == XmlRpc::XmlRpcValue::TypeStruct); 104 | auto &action_list = params["action_list"]; 105 | ROS_ASSERT(action_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 106 | actions_ = new vector(); 107 | 108 | for(int i=0; ipush_back(Action(a["name"], a["onestep_forward_m"], a["onestep_rotation_deg"], i)); 111 | 112 | auto &b = actions_->back(); 113 | ROS_INFO("set an action: %s, %f, %f", b._name.c_str(), b._delta_fw, b._delta_rot); 114 | } 115 | } 116 | 117 | void ViNode::scanReceived(const sensor_msgs::LaserScan::ConstPtr &msg) 118 | { 119 | //vi_->setLocalCost(msg, x_, y_, yaw_); 120 | } 121 | 122 | bool ViNode::servePolicy(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) 123 | { 124 | vi_->policyWriter(response); 125 | return true; 126 | } 127 | 128 | bool ViNode::serveValue(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) 129 | { 130 | vi_->valueFunctionWriter(response); 131 | return true; 132 | } 133 | 134 | void ViNode::executeVi(const value_iteration::ViGoalConstPtr &goal) 135 | { 136 | ROS_INFO("VALUE ITERATION START"); 137 | auto &ori = goal->goal.pose.orientation; 138 | tf::Quaternion q(ori.x, ori.y, ori.z, ori.w); 139 | double roll, pitch, yaw; 140 | tf::Matrix3x3(q).getRPY(roll, pitch, yaw); 141 | int t = (int)(yaw*180/M_PI); 142 | vi_->setGoal(goal->goal.pose.position.x, goal->goal.pose.position.y, t); 143 | 144 | vector ths; 145 | for(int t=0; tthread_num_; t++) 146 | ths.push_back(thread(&ValueIterator::valueIterationWorker, vi_.get(), INT_MAX, t)); 147 | 148 | value_iteration::ViFeedback vi_feedback; 149 | 150 | ros::Rate loop_rate(10); 151 | while(not vi_->finished(vi_feedback.current_sweep_times, vi_feedback.deltas)){ 152 | as_->publishFeedback(vi_feedback); 153 | 154 | if(as_->isPreemptRequested()) 155 | vi_->setCancel(); 156 | 157 | loop_rate.sleep(); 158 | } 159 | as_->publishFeedback(vi_feedback); 160 | 161 | for(auto &th : ths) 162 | th.join(); 163 | 164 | vi_->setCalculated(); 165 | ROS_INFO("VALUE ITERATION END"); 166 | while(online_ and not vi_->endOfTrial() ) 167 | if(as_->isPreemptRequested()){ 168 | vi_->setCancel(); 169 | 170 | loop_rate.sleep(); 171 | } 172 | 173 | ROS_INFO("END OF TRIAL"); 174 | value_iteration::ViResult vi_result; 175 | vi_result.finished = vi_->arrived() or (not online_); 176 | as_->setSucceeded(vi_result); 177 | } 178 | 179 | 180 | void ViNode::pubValueFunction(void) 181 | { 182 | nav_msgs::OccupancyGrid map;//, local_map; 183 | vi_->makeValueFunctionMap(map, cost_drawing_threshold_, x_, y_, yaw_); 184 | pub_value_function_.publish(map); 185 | } 186 | 187 | void ViNode::decision(void) 188 | { 189 | if(not online_) 190 | return; 191 | 192 | try{ 193 | tf::StampedTransform trans; 194 | tf_listener_.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(0.1)); 195 | tf_listener_.lookupTransform("map", "base_link", ros::Time(0), trans); 196 | x_ = trans.getOrigin().x(); 197 | y_ = trans.getOrigin().y(); 198 | yaw_ = tf::getYaw(trans.getRotation()); 199 | }catch(tf::TransformException &e){ 200 | ROS_WARN("%s", e.what()); 201 | } 202 | 203 | //vi_->setLocalWindow(x_, y_); 204 | 205 | geometry_msgs::Twist cmd_vel; 206 | cmd_vel.linear.x = 0.0; 207 | cmd_vel.angular.z = 0.0; 208 | 209 | Action *a = vi_->posToAction(x_, y_, yaw_); 210 | if(a != NULL){ 211 | cmd_vel.linear.x = a->_delta_fw; 212 | cmd_vel.angular.z = a->_delta_rot/180*M_PI; 213 | } 214 | pub_cmd_vel_.publish(cmd_vel); 215 | } 216 | 217 | } 218 | 219 | int main(int argc, char **argv) 220 | { 221 | ros::init(argc,argv,"vi_node"); 222 | value_iteration::ViNode vi_node; 223 | 224 | int step = 0; 225 | ros::Rate loop_rate(10); 226 | while(ros::ok()){ 227 | vi_node.decision(); 228 | 229 | if(step % 30 == 0) 230 | vi_node.pubValueFunction(); 231 | 232 | ros::spinOnce(); 233 | loop_rate.sleep(); 234 | step++; 235 | } 236 | 237 | return 0; 238 | } 239 | -------------------------------------------------------------------------------- /src/vi_node.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/vi_node.h" 2 | 3 | namespace value_iteration{ 4 | 5 | ViNode::ViNode() : private_nh_("~"), yaw_(0.0), x_(0.0), y_(0.0), online_("false") 6 | { 7 | setActions(); 8 | 9 | int thread_num; 10 | private_nh_.param("thread_num", thread_num, 1); 11 | vi_.reset(new ValueIteratorLocal(*actions_, thread_num)); 12 | 13 | private_nh_.param("cost_drawing_threshold", cost_drawing_threshold_, 60); 14 | 15 | setCommunication(); 16 | 17 | nav_msgs::GetMap::Response res; 18 | setMap(res); 19 | } 20 | 21 | ViNode::~ViNode() 22 | { 23 | delete actions_; 24 | } 25 | 26 | void ViNode::setMap(nav_msgs::GetMap::Response &res) 27 | { 28 | int theta_cell_num; 29 | double safety_radius; 30 | double safety_radius_penalty; 31 | double goal_margin_radius; 32 | int goal_margin_theta; 33 | 34 | private_nh_.param("theta_cell_num", theta_cell_num, 60); 35 | private_nh_.param("safety_radius", safety_radius, 0.2); 36 | private_nh_.param("safety_radius_penalty", safety_radius_penalty, 30.0); 37 | private_nh_.param("goal_margin_radius", goal_margin_radius, 0.2); 38 | private_nh_.param("goal_margin_theta", goal_margin_theta, 10); 39 | 40 | std::string map_type; 41 | private_nh_.param("map_type", map_type, std::string("occupancy")); 42 | 43 | if(map_type == "occupancy"){ 44 | while(!ros::service::waitForService("/static_map", ros::Duration(3.0))){ 45 | ROS_INFO("Waiting for static_map"); 46 | } 47 | 48 | ros::ServiceClient client = nh_.serviceClient("/static_map"); 49 | nav_msgs::GetMap::Request req; 50 | if(not client.call(req, res)){ 51 | ROS_ERROR("static_map not working"); 52 | exit(1); 53 | } 54 | 55 | vi_->setMapWithOccupancyGrid(res.map, theta_cell_num, safety_radius, safety_radius_penalty, 56 | goal_margin_radius, goal_margin_theta); 57 | }else if(map_type == "cost"){ 58 | while(!ros::service::waitForService("/cost_map", ros::Duration(3.0))){ 59 | ROS_INFO("Waiting for cost_map"); 60 | } 61 | 62 | ros::ServiceClient client = nh_.serviceClient("/cost_map"); 63 | nav_msgs::GetMap::Request req; 64 | if(not client.call(req, res)){ 65 | ROS_ERROR("cost_map not working"); 66 | exit(1); 67 | } 68 | for(int i=0;i<100;i++) 69 | ROS_INFO("%u", (unsigned int)(res.map.data[i] & 0xFF)); 70 | 71 | vi_->setMapWithCostGrid(res.map, theta_cell_num, safety_radius, safety_radius_penalty, 72 | goal_margin_radius, goal_margin_theta); 73 | }else{ 74 | ROS_ERROR("NO SUPPORT MAP TYPE"); 75 | exit(1); 76 | } 77 | } 78 | 79 | void ViNode::setCommunication(void) 80 | { 81 | private_nh_.param("online", online_, false); 82 | if(online_){ 83 | ROS_INFO("SET ONLINE"); 84 | pub_cmd_vel_ = nh_.advertise("cmd_vel", 2, true); 85 | sub_laser_scan_ = nh_.subscribe("scan", 2, &ViNode::scanReceived, this); 86 | } 87 | 88 | pub_value_function_ = nh_.advertise("value_function", 2, true); 89 | 90 | as_.reset(new actionlib::SimpleActionServer( nh_, "vi_controller", 91 | boost::bind(&ViNode::executeVi, this, _1), false)); 92 | as_->start(); 93 | srv_policy_ = nh_.advertiseService("/policy", &ViNode::servePolicy, this); 94 | srv_value_ = nh_.advertiseService("/value", &ViNode::serveValue, this); 95 | } 96 | 97 | void ViNode::setActions(void) 98 | { 99 | XmlRpc::XmlRpcValue params; 100 | nh_.getParam("/vi_node", params); 101 | ROS_ASSERT(params.getType() == XmlRpc::XmlRpcValue::TypeStruct); 102 | auto &action_list = params["action_list"]; 103 | ROS_ASSERT(action_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 104 | actions_ = new vector(); 105 | 106 | for(int i=0; ipush_back(Action(a["name"], a["onestep_forward_m"], a["onestep_rotation_deg"], i)); 109 | 110 | auto &b = actions_->back(); 111 | ROS_INFO("set an action: %s, %f, %f", b._name.c_str(), b._delta_fw, b._delta_rot); 112 | } 113 | } 114 | 115 | void ViNode::scanReceived(const sensor_msgs::LaserScan::ConstPtr &msg) 116 | { 117 | vi_->setLocalCost(msg, x_, y_, yaw_); 118 | } 119 | 120 | bool ViNode::servePolicy(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) 121 | { 122 | vi_->policyWriter(response); 123 | return true; 124 | } 125 | 126 | bool ViNode::serveValue(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) 127 | { 128 | vi_->valueFunctionWriter(response); 129 | return true; 130 | } 131 | 132 | void ViNode::executeVi(const value_iteration::ViGoalConstPtr &goal) 133 | { 134 | static bool executing = true; 135 | ROS_INFO("VALUE ITERATION START"); 136 | auto &ori = goal->goal.pose.orientation; 137 | tf::Quaternion q(ori.x, ori.y, ori.z, ori.w); 138 | double roll, pitch, yaw; 139 | tf::Matrix3x3(q).getRPY(roll, pitch, yaw); 140 | int t = (int)(yaw*180/M_PI); 141 | vi_->setGoal(goal->goal.pose.position.x, goal->goal.pose.position.y, t); 142 | 143 | vector ths; 144 | for(int t=0; tthread_num_; t++) 145 | ths.push_back(thread(&ValueIterator::valueIterationWorker, vi_.get(), INT_MAX, t)); 146 | 147 | if(online_) 148 | thread(&ValueIteratorLocal::localValueIterationWorker, vi_.get(), 1).detach(); 149 | // for(int t=0;t<1;t++) 150 | // thread(&ValueIteratorLocal::localValueIterationWorker, vi_.get(), t).detach(); 151 | 152 | 153 | value_iteration::ViFeedback vi_feedback; 154 | 155 | ros::Rate loop_rate(10); 156 | while(not vi_->finished(vi_feedback.current_sweep_times, vi_feedback.deltas)){ 157 | as_->publishFeedback(vi_feedback); 158 | 159 | if(as_->isPreemptRequested()) 160 | vi_->setCancel(); 161 | 162 | loop_rate.sleep(); 163 | } 164 | as_->publishFeedback(vi_feedback); 165 | 166 | for(auto &th : ths) 167 | th.join(); 168 | 169 | /* 170 | vi_->setCalculated(); 171 | 172 | ROS_INFO("VALUE ITERATION END"); 173 | for(int t=2;t<4;t++) 174 | thread(&ValueIteratorLocal::localValueIterationWorker, vi_.get(), t).detach(); 175 | */ 176 | 177 | while(not vi_->endOfTrial() ) 178 | if(as_->isPreemptRequested()){ 179 | vi_->setCancel(); 180 | 181 | loop_rate.sleep(); 182 | } 183 | 184 | ROS_INFO("END OF TRIAL"); 185 | value_iteration::ViResult vi_result; 186 | vi_result.finished = vi_->arrived(); 187 | as_->setSucceeded(vi_result); 188 | } 189 | 190 | 191 | void ViNode::pubValueFunction(void) 192 | { 193 | nav_msgs::OccupancyGrid map, local_map; 194 | 195 | vi_->makeValueFunctionMap(map, cost_drawing_threshold_, x_, y_, yaw_); 196 | pub_value_function_.publish(map); 197 | } 198 | 199 | void ViNode::decision(void) 200 | { 201 | if(not online_) 202 | return; 203 | 204 | try{ 205 | tf::StampedTransform trans; 206 | tf_listener_.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(0.1)); 207 | tf_listener_.lookupTransform("map", "base_link", ros::Time(0), trans); 208 | x_ = trans.getOrigin().x(); 209 | y_ = trans.getOrigin().y(); 210 | yaw_ = tf::getYaw(trans.getRotation()); 211 | }catch(tf::TransformException &e){ 212 | ROS_WARN("%s", e.what()); 213 | } 214 | 215 | vi_->setLocalWindow(x_, y_); 216 | 217 | geometry_msgs::Twist cmd_vel; 218 | cmd_vel.linear.x = 0.0; 219 | cmd_vel.angular.z = 0.0; 220 | 221 | Action *a = vi_->posToAction(x_, y_, yaw_); 222 | if(a != NULL){ 223 | cmd_vel.linear.x = a->_delta_fw; 224 | cmd_vel.angular.z = a->_delta_rot/180*M_PI; 225 | } 226 | pub_cmd_vel_.publish(cmd_vel); 227 | } 228 | 229 | } 230 | 231 | int main(int argc, char **argv) 232 | { 233 | ros::init(argc,argv,"vi_node"); 234 | value_iteration::ViNode vi_node; 235 | 236 | int step = 0; 237 | ros::Rate loop_rate(10); 238 | while(ros::ok()){ 239 | vi_node.decision(); 240 | 241 | if(step % 30 == 0) 242 | vi_node.pubValueFunction(); 243 | 244 | ros::spinOnce(); 245 | loop_rate.sleep(); 246 | step++; 247 | } 248 | 249 | return 0; 250 | } 251 | -------------------------------------------------------------------------------- /src/vi_node_after_planning_no_local.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/vi_node_no_local.h" 2 | 3 | namespace value_iteration{ 4 | 5 | ViNode::ViNode() : private_nh_("~"), yaw_(0.0), x_(0.0), y_(0.0), online_("false") 6 | { 7 | setActions(); 8 | 9 | int thread_num; 10 | private_nh_.param("thread_num", thread_num, 1); 11 | vi_.reset(new ValueIterator(*actions_, thread_num)); 12 | 13 | private_nh_.param("cost_drawing_threshold", cost_drawing_threshold_, 60); 14 | 15 | setCommunication(); 16 | 17 | nav_msgs::GetMap::Response res; 18 | setMap(res); 19 | } 20 | 21 | ViNode::~ViNode() 22 | { 23 | delete actions_; 24 | } 25 | 26 | void ViNode::setMap(nav_msgs::GetMap::Response &res) 27 | { 28 | int theta_cell_num; 29 | double safety_radius; 30 | double safety_radius_penalty; 31 | double goal_margin_radius; 32 | int goal_margin_theta; 33 | 34 | private_nh_.param("theta_cell_num", theta_cell_num, 60); 35 | private_nh_.param("safety_radius", safety_radius, 0.2); 36 | private_nh_.param("safety_radius_penalty", safety_radius_penalty, 30.0); 37 | private_nh_.param("goal_margin_radius", goal_margin_radius, 0.2); 38 | private_nh_.param("goal_margin_theta", goal_margin_theta, 10); 39 | 40 | std::string map_type; 41 | private_nh_.param("map_type", map_type, std::string("occupancy")); 42 | 43 | if(map_type == "occupancy"){ 44 | while(!ros::service::waitForService("/static_map", ros::Duration(3.0))){ 45 | ROS_INFO("Waiting for static_map"); 46 | } 47 | 48 | ros::ServiceClient client = nh_.serviceClient("/static_map"); 49 | nav_msgs::GetMap::Request req; 50 | if(not client.call(req, res)){ 51 | ROS_ERROR("static_map not working"); 52 | exit(1); 53 | } 54 | 55 | vi_->setMapWithOccupancyGrid(res.map, theta_cell_num, safety_radius, safety_radius_penalty, 56 | goal_margin_radius, goal_margin_theta); 57 | }else if(map_type == "cost"){ 58 | while(!ros::service::waitForService("/cost_map", ros::Duration(3.0))){ 59 | ROS_INFO("Waiting for cost_map"); 60 | } 61 | 62 | ros::ServiceClient client = nh_.serviceClient("/cost_map"); 63 | nav_msgs::GetMap::Request req; 64 | if(not client.call(req, res)){ 65 | ROS_ERROR("cost_map not working"); 66 | exit(1); 67 | } 68 | for(int i=0;i<100;i++) 69 | ROS_INFO("%u", (unsigned int)(res.map.data[i] & 0xFF)); 70 | 71 | vi_->setMapWithCostGrid(res.map, theta_cell_num, safety_radius, safety_radius_penalty, 72 | goal_margin_radius, goal_margin_theta); 73 | }else{ 74 | ROS_ERROR("NO SUPPORT MAP TYPE"); 75 | exit(1); 76 | } 77 | } 78 | 79 | void ViNode::setCommunication(void) 80 | { 81 | private_nh_.param("online", online_, false); 82 | if(online_){ 83 | ROS_INFO("SET ONLINE"); 84 | pub_cmd_vel_ = nh_.advertise("cmd_vel", 2, true); 85 | //sub_pose_ = nh_.subscribe("mcl_pose", 2, &ViNode::poseReceived, this); 86 | sub_laser_scan_ = nh_.subscribe("scan", 2, &ViNode::scanReceived, this); 87 | } 88 | 89 | pub_value_function_ = nh_.advertise("value_function", 2, true); 90 | //pub_local_value_function_ = nh_.advertise("local_value_function", 2, true); 91 | 92 | as_.reset(new actionlib::SimpleActionServer( nh_, "vi_controller", 93 | boost::bind(&ViNode::executeVi, this, _1), false)); 94 | as_->start(); 95 | srv_policy_ = nh_.advertiseService("/policy", &ViNode::servePolicy, this); 96 | srv_value_ = nh_.advertiseService("/value", &ViNode::serveValue, this); 97 | } 98 | 99 | void ViNode::setActions(void) 100 | { 101 | XmlRpc::XmlRpcValue params; 102 | nh_.getParam("/vi_node", params); 103 | ROS_ASSERT(params.getType() == XmlRpc::XmlRpcValue::TypeStruct); 104 | auto &action_list = params["action_list"]; 105 | ROS_ASSERT(action_list.getType() == XmlRpc::XmlRpcValue::TypeArray); 106 | actions_ = new vector(); 107 | 108 | for(int i=0; ipush_back(Action(a["name"], a["onestep_forward_m"], a["onestep_rotation_deg"], i)); 111 | 112 | auto &b = actions_->back(); 113 | ROS_INFO("set an action: %s, %f, %f", b._name.c_str(), b._delta_fw, b._delta_rot); 114 | } 115 | } 116 | 117 | void ViNode::scanReceived(const sensor_msgs::LaserScan::ConstPtr &msg) 118 | { 119 | //vi_->setLocalCost(msg, x_, y_, yaw_); 120 | } 121 | 122 | bool ViNode::servePolicy(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) 123 | { 124 | vi_->policyWriter(response); 125 | return true; 126 | } 127 | 128 | bool ViNode::serveValue(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) 129 | { 130 | vi_->valueFunctionWriter(response); 131 | return true; 132 | } 133 | 134 | void ViNode::executeVi(const value_iteration::ViGoalConstPtr &goal) 135 | { 136 | ROS_INFO("VALUE ITERATION START"); 137 | auto &ori = goal->goal.pose.orientation; 138 | tf::Quaternion q(ori.x, ori.y, ori.z, ori.w); 139 | double roll, pitch, yaw; 140 | tf::Matrix3x3(q).getRPY(roll, pitch, yaw); 141 | int t = (int)(yaw*180/M_PI); 142 | vi_->setGoal(goal->goal.pose.position.x, goal->goal.pose.position.y, t); 143 | 144 | vector ths; 145 | for(int t=0; tthread_num_; t++) 146 | ths.push_back(thread(&ValueIterator::valueIterationWorker, vi_.get(), INT_MAX, t)); 147 | 148 | value_iteration::ViFeedback vi_feedback; 149 | 150 | ros::Rate loop_rate(10); 151 | while(not vi_->finished(vi_feedback.current_sweep_times, vi_feedback.deltas)){ 152 | as_->publishFeedback(vi_feedback); 153 | 154 | if(as_->isPreemptRequested()) 155 | vi_->setCancel(); 156 | 157 | loop_rate.sleep(); 158 | } 159 | as_->publishFeedback(vi_feedback); 160 | 161 | for(auto &th : ths) 162 | th.join(); 163 | 164 | //vi_->copyFromGlobal(); 165 | vi_->setCalculated(); 166 | 167 | ROS_INFO("VALUE ITERATION END"); 168 | 169 | while(online_ and not vi_->endOfTrial() ) 170 | if(as_->isPreemptRequested()){ 171 | vi_->setCancel(); 172 | 173 | loop_rate.sleep(); 174 | } 175 | 176 | ROS_INFO("END OF TRIAL"); 177 | value_iteration::ViResult vi_result; 178 | vi_result.finished = vi_->arrived() or (not online_); 179 | as_->setSucceeded(vi_result); 180 | } 181 | 182 | 183 | void ViNode::pubValueFunction(void) 184 | { 185 | nav_msgs::OccupancyGrid map; 186 | 187 | vi_->makeValueFunctionMap(map, cost_drawing_threshold_, x_, y_, yaw_); 188 | pub_value_function_.publish(map); 189 | 190 | //vi_->makeLocalValueFunctionMap(local_map, cost_drawing_threshold_, x_, y_, yaw_); 191 | //pub_local_value_function_.publish(local_map); 192 | } 193 | 194 | void ViNode::decision(void) 195 | { 196 | if(not vi_->isCalculated()) 197 | return; 198 | 199 | try{ 200 | tf::StampedTransform trans; 201 | tf_listener_.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(0.1)); 202 | tf_listener_.lookupTransform("map", "base_link", ros::Time(0), trans); 203 | x_ = trans.getOrigin().x(); 204 | y_ = trans.getOrigin().y(); 205 | yaw_ = tf::getYaw(trans.getRotation()); 206 | }catch(tf::TransformException &e){ 207 | ROS_WARN("%s", e.what()); 208 | } 209 | 210 | //vi_->setLocalWindow(x_, y_); 211 | 212 | geometry_msgs::Twist cmd_vel; 213 | cmd_vel.linear.x = 0.0; 214 | cmd_vel.angular.z = 0.0; 215 | 216 | Action *a = vi_->posToAction(x_, y_, yaw_); 217 | if(a != NULL){ 218 | cmd_vel.linear.x = a->_delta_fw; 219 | cmd_vel.angular.z = a->_delta_rot/180*M_PI; 220 | } 221 | pub_cmd_vel_.publish(cmd_vel); 222 | } 223 | 224 | } 225 | 226 | int main(int argc, char **argv) 227 | { 228 | ros::init(argc,argv,"vi_node"); 229 | value_iteration::ViNode vi_node; 230 | 231 | int step = 0; 232 | ros::Rate loop_rate(10); 233 | while(ros::ok()){ 234 | vi_node.decision(); 235 | 236 | if(step % 30 == 0) 237 | vi_node.pubValueFunction(); 238 | 239 | ros::spinOnce(); 240 | loop_rate.sleep(); 241 | step++; 242 | } 243 | 244 | return 0; 245 | } 246 | -------------------------------------------------------------------------------- /config/conf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 138 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | - /TF1/Tree1 9 | - /Map1 10 | Splitter Ratio: 0.5 11 | Tree Height: 1507 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: LaserScan 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 20 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Class: rviz/RobotModel 57 | Collision Enabled: false 58 | Enabled: true 59 | Links: 60 | All Links Enabled: true 61 | Expand Joint Details: false 62 | Expand Link Details: false 63 | Expand Tree: false 64 | Link Tree Style: Links in Alphabetic Order 65 | base_footprint: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | base_link: 70 | Alpha: 1 71 | Show Axes: false 72 | Show Trail: false 73 | Value: true 74 | base_scan: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | Value: true 79 | caster_back_link: 80 | Alpha: 1 81 | Show Axes: false 82 | Show Trail: false 83 | Value: true 84 | imu_link: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | wheel_left_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | wheel_right_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | Name: RobotModel 99 | Robot Description: robot_description 100 | TF Prefix: "" 101 | Update Interval: 0 102 | Value: true 103 | Visual Enabled: true 104 | - Class: rviz/TF 105 | Enabled: false 106 | Frame Timeout: 15 107 | Frames: 108 | All Enabled: false 109 | Marker Alpha: 1 110 | Marker Scale: 1 111 | Name: TF 112 | Show Arrows: true 113 | Show Axes: true 114 | Show Names: false 115 | Tree: 116 | {} 117 | Update Interval: 0 118 | Value: false 119 | - Alpha: 1 120 | Autocompute Intensity Bounds: true 121 | Autocompute Value Bounds: 122 | Max Value: 10 123 | Min Value: -10 124 | Value: true 125 | Axis: Z 126 | Channel Name: intensity 127 | Class: rviz/LaserScan 128 | Color: 0; 255; 0 129 | Color Transformer: FlatColor 130 | Decay Time: 0 131 | Enabled: true 132 | Invert Rainbow: false 133 | Max Color: 255; 255; 255 134 | Min Color: 0; 0; 0 135 | Name: LaserScan 136 | Position Transformer: XYZ 137 | Queue Size: 10 138 | Selectable: true 139 | Size (Pixels): 3 140 | Size (m): 0.030000001192092896 141 | Style: Flat Squares 142 | Topic: /scan 143 | Unreliable: false 144 | Use Fixed Frame: true 145 | Use rainbow: true 146 | Value: true 147 | - Class: rviz/Image 148 | Enabled: false 149 | Image Topic: /raspicam_node/image 150 | Max Value: 1 151 | Median window: 5 152 | Min Value: 0 153 | Name: Image 154 | Normalize Range: true 155 | Queue Size: 2 156 | Transport Hint: compressed 157 | Unreliable: false 158 | Value: false 159 | - Alpha: 1 160 | Class: rviz/Map 161 | Color Scheme: costmap 162 | Draw Behind: false 163 | Enabled: true 164 | Name: Map 165 | Topic: /value_function 166 | Unreliable: false 167 | Use Timestamp: false 168 | Value: true 169 | - Alpha: 1 170 | Buffer Length: 1 171 | Class: rviz/Path 172 | Color: 0; 0; 0 173 | Enabled: true 174 | Head Diameter: 0.30000001192092896 175 | Head Length: 0.20000000298023224 176 | Length: 0.30000001192092896 177 | Line Style: Lines 178 | Line Width: 0.029999999329447746 179 | Name: Planner Plan 180 | Offset: 181 | X: 0 182 | Y: 0 183 | Z: 0 184 | Pose Color: 255; 85; 255 185 | Pose Style: None 186 | Queue Size: 10 187 | Radius: 0.029999999329447746 188 | Shaft Diameter: 0.10000000149011612 189 | Shaft Length: 0.10000000149011612 190 | Topic: /move_base/NavfnROS/plan 191 | Unreliable: false 192 | Value: true 193 | - Alpha: 1 194 | Arrow Length: 0.05000000074505806 195 | Axes Length: 0.30000001192092896 196 | Axes Radius: 0.009999999776482582 197 | Class: rviz/PoseArray 198 | Color: 0; 192; 0 199 | Enabled: true 200 | Head Length: 0.07000000029802322 201 | Head Radius: 0.029999999329447746 202 | Name: Amcl Particles 203 | Queue Size: 10 204 | Shaft Length: 0.23000000417232513 205 | Shaft Radius: 0.009999999776482582 206 | Shape: Arrow (Flat) 207 | Topic: /particlecloud 208 | Unreliable: false 209 | Value: true 210 | - Alpha: 1 211 | Axes Length: 1 212 | Axes Radius: 0.10000000149011612 213 | Class: rviz/Pose 214 | Color: 255; 25; 0 215 | Enabled: true 216 | Head Length: 0.30000001192092896 217 | Head Radius: 0.10000000149011612 218 | Name: Goal 219 | Queue Size: 10 220 | Shaft Length: 0.5 221 | Shaft Radius: 0.05000000074505806 222 | Shape: Arrow 223 | Topic: /move_base_simple/goal 224 | Unreliable: false 225 | Value: true 226 | Enabled: true 227 | Global Options: 228 | Background Color: 48; 48; 48 229 | Default Light: true 230 | Fixed Frame: map 231 | Frame Rate: 30 232 | Name: root 233 | Tools: 234 | - Class: rviz/MoveCamera 235 | - Class: rviz/Interact 236 | Hide Inactive Objects: true 237 | - Class: rviz/Select 238 | - Class: rviz/SetInitialPose 239 | Theta std deviation: 0.2617993950843811 240 | Topic: /initialpose 241 | X std deviation: 0.5 242 | Y std deviation: 0.5 243 | - Class: rviz/SetGoal 244 | Topic: /move_base_simple/goal 245 | - Class: rviz/Measure 246 | Value: true 247 | Views: 248 | Current: 249 | Angle: -1.5707963705062866 250 | Class: rviz/TopDownOrtho 251 | Enable Stereo Rendering: 252 | Stereo Eye Separation: 0.05999999865889549 253 | Stereo Focal Distance: 1 254 | Swap Stereo Eyes: false 255 | Value: false 256 | Invert Z Axis: false 257 | Name: Current View 258 | Near Clip Distance: 0.009999999776482582 259 | Scale: 100 260 | Target Frame: 261 | X: 0 262 | Y: 0 263 | Saved: ~ 264 | Window Geometry: 265 | Displays: 266 | collapsed: false 267 | Height: 1911 268 | Hide Left Dock: false 269 | Hide Right Dock: true 270 | Image: 271 | collapsed: false 272 | QMainWindow State: 000000ff00000000fd0000000400000000000001f7000006dbfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006db0000018200fffffffb0000000a0049006d006100670065000000067d000000cc0000002600fffffffb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a00000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000008d800fffffffb0000000800540069006d00650100000000000004500000000000000000000005e7000006db00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 273 | Selection: 274 | collapsed: false 275 | Time: 276 | collapsed: false 277 | Tool Properties: 278 | collapsed: false 279 | Views: 280 | collapsed: true 281 | Width: 2026 282 | X: 16 283 | Y: 0 284 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(value_iteration) 3 | 4 | set(CMAKE_CXX_FLAGS "-std=c++14 -O3 ${CMAKE_CXX_FLAGS}") 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | 10 | find_package(Eigen3 REQUIRED) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | rospy 14 | roscpp 15 | std_msgs 16 | actionlib 17 | actionlib_msgs 18 | genmsg 19 | geometry_msgs 20 | message_generation 21 | cv_bridge 22 | grid_map_core 23 | grid_map_ros 24 | grid_map_cv 25 | grid_map_filters 26 | grid_map_loader 27 | grid_map_msgs 28 | grid_map_octomap 29 | grid_map_rviz_plugin 30 | grid_map_visualization 31 | filters 32 | ) 33 | 34 | ## System dependencies are found with CMake's conventions 35 | # find_package(Boost REQUIRED COMPONENTS system) 36 | 37 | 38 | ## Uncomment this if the package has a setup.py. This macro ensures 39 | ## modules and global scripts declared therein get installed 40 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 41 | # catkin_python_setup() 42 | 43 | ################################################ 44 | ## Declare ROS messages, services and actions ## 45 | ################################################ 46 | 47 | ## To declare and build messages, services or actions from within this 48 | ## package, follow these steps: 49 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 50 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 51 | ## * In the file package.xml: 52 | ## * add a build_depend tag for "message_generation" 53 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 54 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 55 | ## but can be declared for certainty nonetheless: 56 | ## * add a run_depend tag for "message_runtime" 57 | ## * In this file (CMakeLists.txt): 58 | ## * add "message_generation" and every package in MSG_DEP_SET to 59 | ## find_package(catkin REQUIRED COMPONENTS ...) 60 | ## * add "message_runtime" and every package in MSG_DEP_SET to 61 | ## catkin_package(CATKIN_DEPENDS ...) 62 | ## * uncomment the add_*_files sections below as needed 63 | ## and list every .msg/.srv/.action file to be processed 64 | ## * uncomment the generate_messages entry below 65 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 66 | 67 | ## Generate messages in the 'msg' folder 68 | # add_message_files( 69 | # FILES 70 | # Message1.msg 71 | # Message2.msg 72 | # ) 73 | 74 | ## Generate services in the 'srv' folder 75 | #add_service_files( 76 | # FILES 77 | # EventRegist.srv 78 | # ActionRegist.srv 79 | # FlushData.srv 80 | #) 81 | 82 | # Generate actions in the 'action' folder 83 | add_action_files( 84 | FILES 85 | Vi.action 86 | ) 87 | 88 | ## Generate added messages and services with any dependencies listed here 89 | generate_messages( 90 | DEPENDENCIES 91 | std_msgs 92 | geometry_msgs 93 | actionlib_msgs 94 | ) 95 | 96 | ################################################ 97 | ## Declare ROS dynamic reconfigure parameters ## 98 | ################################################ 99 | 100 | ## To declare and build dynamic reconfigure parameters within this 101 | ## package, follow these steps: 102 | ## * In the file package.xml: 103 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 104 | ## * In this file (CMakeLists.txt): 105 | ## * add "dynamic_reconfigure" to 106 | ## find_package(catkin REQUIRED COMPONENTS ...) 107 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 108 | ## and list every .cfg file to be processed 109 | 110 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 111 | # generate_dynamic_reconfigure_options( 112 | # cfg/DynReconf1.cfg 113 | # cfg/DynReconf2.cfg 114 | # ) 115 | 116 | ################################### 117 | ## catkin specific configuration ## 118 | ################################### 119 | ## The catkin_package macro generates cmake config files for your package 120 | ## Declare things to be passed to dependent projects 121 | ## INCLUDE_DIRS: uncomment this if you package contains header files 122 | ## LIBRARIES: libraries you create in this project that dependent projects also need 123 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 124 | ## DEPENDS: system dependencies of this project that dependent projects also need 125 | catkin_package( 126 | INCLUDE_DIRS include 127 | LIBRARIES value_iteration 128 | CATKIN_DEPENDS roscpp rospy message_runtime #cv_bridge 129 | #DEPENDS system_lib 130 | grid_map_core 131 | grid_map_ros 132 | grid_map_cv 133 | grid_map_filters 134 | grid_map_loader 135 | grid_map_msgs 136 | grid_map_octomap 137 | grid_map_rviz_plugin 138 | grid_map_visualization 139 | ) 140 | 141 | ########### 142 | ## Build ## 143 | ########### 144 | 145 | ## Specify additional locations of header files 146 | ## Your package locations should be listed before other locations 147 | # include_directories(include) 148 | include_directories( 149 | include 150 | ${EIGEN3_INCLUDE_DIR} 151 | ${catkin_INCLUDE_DIRS} 152 | ) 153 | 154 | ## Declare a C++ library 155 | add_library(ValueIterator src/ValueIterator.cpp) 156 | add_library(ValueIteratorLocal src/ValueIteratorLocal.cpp) 157 | add_library(SweepWorkerStatus src/SweepWorkerStatus.cpp) 158 | add_library(State src/State.cpp) 159 | add_library(Action src/Action.cpp) 160 | add_library(StateTransition src/StateTransition.cpp) 161 | 162 | # この2行を追加 163 | add_dependencies(ValueIterator ${TARGET_NAME} ${PROJECT_NAME}_generate_messages_cpp) 164 | add_dependencies(ValueIteratorLocal ${TARGET_NAME} ${PROJECT_NAME}_generate_messages_cpp) 165 | add_dependencies(SweepWorkerStatus ${TARGET_NAME} ${PROJECT_NAME}_generate_messages_cpp) 166 | add_dependencies(State ${TARGET_NAME} ${PROJECT_NAME}_generate_messages_cpp) 167 | add_dependencies(Action ${TARGET_NAME} ${PROJECT_NAME}_generate_messages_cpp) 168 | add_dependencies(StateTransition ${TARGET_NAME} ${PROJECT_NAME}_generate_messages_cpp) 169 | 170 | ## Add cmake target dependencies of the library 171 | ## as an example, code may need to be generated before libraries 172 | ## either from message generation or dynamic reconfigure 173 | #add_dependencies(vi_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 174 | #add_dependencies(vi_node vi_node_generate_messages_cpp) 175 | 176 | ## Declare a C++ executable 177 | add_executable(vi_node src/vi_node.cpp) 178 | add_executable(vi_node_no_local src/vi_node_no_local.cpp) 179 | add_executable(vi_node_after_planning src/vi_node_after_planning.cpp) 180 | add_executable(vi_node_after_planning_no_local src/vi_node_after_planning_no_local.cpp) 181 | 182 | ## Add cmake target dependencies of the executable 183 | ## same as for the library above 184 | add_dependencies(vi_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 185 | add_dependencies(vi_node_no_local ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 186 | add_dependencies(vi_node_after_planning ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 187 | add_dependencies(vi_node_after_planning_no_local ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 188 | 189 | ## Specify libraries to link a library or executable target against 190 | target_link_libraries(vi_node ${catkin_LIBRARIES} ) 191 | target_link_libraries(vi_node ValueIterator ${catkin_LIBRARIES} ) 192 | target_link_libraries(vi_node ValueIteratorLocal ${catkin_LIBRARIES} ) 193 | target_link_libraries(vi_node SweepWorkerStatus ${catkin_LIBRARIES} ) 194 | target_link_libraries(vi_node State ${catkin_LIBRARIES} ) 195 | target_link_libraries(vi_node Action ${catkin_LIBRARIES} ) 196 | target_link_libraries(vi_node StateTransition ${catkin_LIBRARIES} ) 197 | 198 | ## Specify libraries to link a library or executable target against 199 | target_link_libraries(vi_node_after_planning ${catkin_LIBRARIES} ) 200 | target_link_libraries(vi_node_after_planning ValueIterator ${catkin_LIBRARIES} ) 201 | target_link_libraries(vi_node_after_planning ValueIteratorLocal ${catkin_LIBRARIES} ) 202 | target_link_libraries(vi_node_after_planning SweepWorkerStatus ${catkin_LIBRARIES} ) 203 | target_link_libraries(vi_node_after_planning State ${catkin_LIBRARIES} ) 204 | target_link_libraries(vi_node_after_planning Action ${catkin_LIBRARIES} ) 205 | target_link_libraries(vi_node_after_planning StateTransition ${catkin_LIBRARIES} ) 206 | 207 | ## Specify libraries to link a library or executable target against 208 | target_link_libraries(vi_node_no_local ${catkin_LIBRARIES} ) 209 | target_link_libraries(vi_node_no_local ValueIterator ${catkin_LIBRARIES} ) 210 | target_link_libraries(vi_node_no_local SweepWorkerStatus ${catkin_LIBRARIES} ) 211 | target_link_libraries(vi_node_no_local State ${catkin_LIBRARIES} ) 212 | target_link_libraries(vi_node_no_local Action ${catkin_LIBRARIES} ) 213 | target_link_libraries(vi_node_no_local StateTransition ${catkin_LIBRARIES} ) 214 | 215 | ## Specify libraries to link a library or executable target against 216 | target_link_libraries(vi_node_after_planning_no_local ${catkin_LIBRARIES} ) 217 | target_link_libraries(vi_node_after_planning_no_local ValueIterator ${catkin_LIBRARIES} ) 218 | target_link_libraries(vi_node_after_planning_no_local SweepWorkerStatus ${catkin_LIBRARIES} ) 219 | target_link_libraries(vi_node_after_planning_no_local State ${catkin_LIBRARIES} ) 220 | target_link_libraries(vi_node_after_planning_no_local Action ${catkin_LIBRARIES} ) 221 | target_link_libraries(vi_node_after_planning_no_local StateTransition ${catkin_LIBRARIES} ) 222 | 223 | ############# 224 | ## Install ## 225 | ############# 226 | 227 | # all install targets should use catkin DESTINATION variables 228 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 229 | 230 | ## Mark executable scripts (Python etc.) for installation 231 | ## in contrast to setup.py, you can choose the destination 232 | # install(PROGRAMS 233 | # scripts/my_python_script 234 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 235 | # ) 236 | 237 | # Mark executables and/or libraries for installation 238 | install(TARGETS vi_node vi_node_no_local 239 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 240 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 241 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 242 | ) 243 | 244 | # Mark cpp header files for installation 245 | install(DIRECTORY include/${PROJECT_NAME}/ 246 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 247 | FILES_MATCHING PATTERN "*.h" 248 | PATTERN ".svn" EXCLUDE 249 | ) 250 | 251 | ## Mark other files for installation (e.g. launch and bag files, etc.) 252 | # install(FILES 253 | # # myfile1 254 | # # myfile2 255 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 256 | # ) 257 | -------------------------------------------------------------------------------- /src/ValueIterator.cpp: -------------------------------------------------------------------------------- 1 | #include "value_iteration/ValueIterator.h" 2 | #include 3 | #include 4 | #include 5 | 6 | namespace value_iteration{ 7 | 8 | ValueIterator::ValueIterator(std::vector &actions, int thread_num) 9 | : actions_(actions), thread_num_(thread_num), status_("init"), 10 | goal_x_(0.0), goal_y_(0.0), goal_t_(0) 11 | { 12 | } 13 | 14 | void ValueIterator::setMapWithOccupancyGrid(nav_msgs::OccupancyGrid &map, int theta_cell_num, 15 | double safety_radius, double safety_radius_penalty, 16 | double goal_margin_radius, int goal_margin_theta) 17 | { 18 | cell_num_t_ = theta_cell_num; 19 | goal_margin_radius_ = goal_margin_radius; 20 | goal_margin_theta_ = goal_margin_theta; 21 | 22 | cell_num_x_ = map.info.width; 23 | cell_num_y_ = map.info.height; 24 | 25 | xy_resolution_ = map.info.resolution; 26 | t_resolution_ = 360/cell_num_t_; 27 | 28 | map_origin_x_ = map.info.origin.position.x; 29 | map_origin_y_ = map.info.origin.position.y; 30 | map_origin_quat_ = map.info.origin.orientation; 31 | 32 | ROS_INFO("SET STATES START"); 33 | setState(map, safety_radius, safety_radius_penalty); 34 | setStateTransition(); 35 | setSweepOrders(); 36 | ROS_INFO("SET STATES END"); 37 | } 38 | 39 | void ValueIterator::setMapWithCostGrid(nav_msgs::OccupancyGrid &map, int theta_cell_num, 40 | double safety_radius, double safety_radius_penalty, 41 | double goal_margin_radius, int goal_margin_theta) 42 | { 43 | cell_num_t_ = theta_cell_num; 44 | goal_margin_radius_ = goal_margin_radius; 45 | goal_margin_theta_ = goal_margin_theta; 46 | 47 | cell_num_x_ = map.info.width; 48 | cell_num_y_ = map.info.height; 49 | 50 | xy_resolution_ = map.info.resolution; 51 | t_resolution_ = 360/cell_num_t_; 52 | 53 | map_origin_x_ = map.info.origin.position.x; 54 | map_origin_y_ = map.info.origin.position.y; 55 | map_origin_quat_ = map.info.origin.orientation; 56 | 57 | states_.clear(); 58 | int margin = (int)ceil(safety_radius/xy_resolution_); 59 | 60 | for(int y=0; y theta_state_transitions; 88 | for(auto &a : actions_) 89 | for(int t=0; t ths; 93 | for(int t=0; t 0 ? delta : -delta; 183 | } 184 | 185 | void ValueIterator::valueIterationWorker(int times, int id) 186 | { 187 | thread_status_.insert(make_pair(id, SweepWorkerStatus())); 188 | 189 | for(int j=0; j> prob_base_bit_); 197 | if(/*thread_status_[id]._delta < 0.1 or */status_ == "canceled" or status_ == "goal" ) 198 | break; 199 | } 200 | 201 | thread_status_[id]._finished = true; 202 | } 203 | 204 | int ValueIterator::toIndex(int ix, int iy, int it) 205 | { 206 | return it + ix*cell_num_t_ + iy*(cell_num_t_*cell_num_x_); 207 | } 208 | 209 | bool ValueIterator::inMapArea(int ix, int iy) 210 | { 211 | return ix >= 0 and ix < cell_num_x_ and iy >= 0 and iy < cell_num_y_; 212 | } 213 | 214 | uint64_t ValueIterator::actionCost(State &s, Action &a) 215 | { 216 | uint64_t cost = 0; 217 | for(auto &tran : a._state_transitions[s.it_]){ 218 | int ix = s.ix_ + tran._dix; 219 | if(ix < 0 or ix >= cell_num_x_) 220 | return max_cost_; 221 | 222 | int iy = s.iy_ + tran._diy; 223 | if(iy < 0 or iy >= cell_num_y_) 224 | return max_cost_; 225 | 226 | int it = (tran._dit + cell_num_t_)%cell_num_t_; 227 | 228 | auto &after_s = states_[toIndex(ix, iy, it)]; 229 | if(not after_s.free_) 230 | return max_cost_; 231 | 232 | cost += ( after_s.total_cost_ + after_s.penalty_ + after_s.local_penalty_ ) * tran._prob; 233 | } 234 | 235 | return cost >> prob_base_bit_; 236 | } 237 | 238 | void ValueIterator::setState(const nav_msgs::OccupancyGrid &map, double safety_radius, double safety_radius_penalty) 239 | { 240 | states_.clear(); 241 | int margin = (int)ceil(safety_radius/xy_resolution_); 242 | 243 | for(int y=0; y 180 ? goal_t_ - 360 : goal_t_ + 360; 269 | 270 | s.final_state_ &= 271 | (goal_t_ - goal_margin_theta_ <= t0 and t1 <= goal_t_ + goal_margin_theta_) or 272 | (goal_t_2 - goal_margin_theta_ <= t0 and t1 <= goal_t_2 + goal_margin_theta_); 273 | } 274 | 275 | for(auto &s : states_){ 276 | //s.renew_ = false; 277 | s.total_cost_ = s.final_state_ ? 0 : max_cost_; 278 | //s.local_total_cost_ = s.total_cost_; 279 | s.local_penalty_ = 0; 280 | s.optimal_action_ = NULL; 281 | //s.local_optimal_action_ = NULL; 282 | } 283 | } 284 | 285 | bool ValueIterator::valueFunctionWriter(grid_map_msgs::GetGridMap::Response& response) 286 | { 287 | grid_map::GridMap map; 288 | map.setFrameId("map"); 289 | map.setGeometry(grid_map::Length(cell_num_x_*xy_resolution_, cell_num_y_*xy_resolution_), xy_resolution_); 290 | 291 | for(int t=0; tid_; 322 | } 323 | } 324 | 325 | grid_map_msgs::GridMap message; 326 | grid_map::GridMapRosConverter::toMessage(map, message); 327 | response.map = message; 328 | 329 | return true; 330 | } 331 | 332 | 333 | void ValueIterator::setGoal(double goal_x, double goal_y, int goal_t) 334 | { 335 | while(goal_t < 0) 336 | goal_t += 360; 337 | while(goal_t >= 360) 338 | goal_t -= 360; 339 | 340 | goal_x_ = goal_x; 341 | goal_y_ = goal_y; 342 | goal_t_ = goal_t; 343 | 344 | ROS_INFO("GOAL: %f, %f, %d", goal_x_, goal_y_, goal_t_); 345 | 346 | thread_status_.clear(); 347 | setStateValues(); 348 | status_ = "calculating"; 349 | } 350 | 351 | void ValueIterator::makeValueFunctionMap(nav_msgs::OccupancyGrid &map, int threshold, 352 | double x, double y, double yaw_rad) 353 | { 354 | map.header.stamp = ros::Time::now(); 355 | map.header.frame_id = "map"; 356 | map.info.resolution = xy_resolution_; 357 | map.info.width = cell_num_x_; 358 | map.info.height = cell_num_y_; 359 | map.info.origin.position.x = map_origin_x_; 360 | map.info.origin.position.y = map_origin_y_; 361 | map.info.origin.orientation = map_origin_quat_; 362 | 363 | int it = (int)floor( ( ((int)(yaw_rad/M_PI*180) + 360*100)%360 )/t_resolution_ ); 364 | 365 | for(int y=0; y() ); 387 | for(int y=0; y() ); 393 | for(int x=0; x