Skip to content

Commit 4251b51

Browse files
globeofgeekglobeofgeek
committed
random wander functions, along with ability to determine if robot is stuck (using navigation_adapter).
1 parent 2adc83a commit 4251b51

File tree

14 files changed

+221
-12
lines changed

14 files changed

+221
-12
lines changed

‎arc_behaviour/CMakeLists.txt‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -121,6 +121,7 @@ catkin_package(
121121
# include_directories(include)
122122
include_directories(
123123
${catkin_INCLUDE_DIRS}
124+
include
124125
)
125126

126127
## Declare a C++ library
@@ -192,7 +193,7 @@ include_directories(
192193

193194
## Add folders to be run by python nosetests
194195
# catkin_add_nosetests(test)
195-
add_executable(arc_base src/arc_base_node.cpp src/ArcBase.cpp include/ArcBase.h)
196+
add_executable(arc_base src/arc_base_node.cpp src/ArcBase.cpp include/ArcBase.h src/RandomWanderMS.cpp src/RandomWanderMS.h)
196197

197198
target_link_libraries(arc_base
198199
${catkin_LIBRARIES}
@@ -204,14 +205,17 @@ add_executable(detect_robot_ps src/detect_robot_ps_node.cpp src/DetectRobotPS.cp
204205
add_executable(detect_debris_ps src/detect_debris_ps_node.cpp src/DetectDebrisPS.cpp include/DetectDebrisPS.h)
205206
add_executable(detect_victim_ps src/detect_victim_ps_node.cpp src/DetectVictimPS.cpp include/DetectVictimPS.h)
206207
add_executable(navigation_adapter src/NavigationAdapter.cpp include/NavigationAdapter.h src/navigation_adapter_node.cpp)
208+
add_executable(random_wander_ms src/RandomWanderMS.cpp src/RandomWanderMS.h src/random_wander_ms_node.cpp)
207209

208210
target_link_libraries(detect_victim_ps ${catkin_LIBRARIES} )
209211
target_link_libraries(detect_marker_ps ${catkin_LIBRARIES} )
210212
target_link_libraries(detect_robot_ps ${catkin_LIBRARIES} )
211213
target_link_libraries(detect_debris_ps ${catkin_LIBRARIES} )
214+
target_link_libraries(random_wander_ms ${catkin_LIBRARIES} )
212215
target_link_libraries(navigation_adapter ${catkin_LIBRARIES} )
213216

214217
add_dependencies(detect_debris_ps marker_msgs_generate_messages_cpp arc_msgs_generate_messages_cpp)
215218
add_dependencies(detect_victim_ps marker_msgs_generate_messages_cpp arc_msgs_generate_messages_cpp)
216219
add_dependencies(detect_robot_ps marker_msgs_generate_messages_cpp arc_msgs_generate_messages_cpp)
220+
add_dependencies(random_wander_ms marker_msgs_generate_messages_cpp arc_msgs_generate_messages_cpp)
217221
add_dependencies(navigation_adapter arc_msgs_generate_messages_cpp)

‎arc_behaviour/include/NavigationAdapter.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
#include "arc_msgs/NavigationRequest.h"
1818
#include <move_base_msgs/MoveBaseAction.h>
1919
#include <actionlib/client/simple_action_client.h>
20+
#include "std_srvs/Trigger.h"
2021

2122
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
2223
namespace arc_behaviour {
@@ -32,6 +33,10 @@ namespace arc_behaviour {
3233
*/
3334
ros::ServiceServer move_to_goal_server;
3435

36+
/**
37+
* Used to determine if robot is currently stuck (ie it won't make it to any navigation goals)
38+
*/
39+
ros::ServiceServer is_stuck_server;
3540
/**
3641
* Our communication with the navigation stack.
3742
*/
@@ -47,6 +52,8 @@ namespace arc_behaviour {
4752
*/
4853
int current_nav_priority;
4954

55+
bool is_stuck = false; //set to true if we tried reaching goal but received fail/stuck signal back from navigation stack.
56+
5057
/**
5158
* Process navigation request and send it to be handled by navigation stack.
5259
* @param req: The accepted navigation request.
@@ -61,6 +68,12 @@ namespace arc_behaviour {
6168
*/
6269
bool move_to_goal_request_cb(arc_msgs::NavigationRequest::Request &req, arc_msgs::NavigationRequest::Response &res);
6370

71+
/**
72+
* Check if robot is stuck currently.
73+
* @return True if stuck, false otherwise.
74+
*/
75+
bool is_stuck_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
76+
6477
/**
6578
* When move_base is done, it will send a callback to signal the result of navigation. This may be either when the navigation is completed
6679
* succesfully or it fails to navigate.

‎arc_behaviour/launch/default.launch‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,4 +16,9 @@
1616
<node name="navigation_adapter" pkg="arc_behaviour" type="navigation_adapter">
1717
</node>
1818

19+
<node name="random_wander_ms" pkg="arc_behaviour" type="random_wander_ms">
20+
<param name="max_range" value="15"/>
21+
<param name="update_goal_freq" value="0.05"/>
22+
<param name="priority" value="1"/>
23+
</node>
1924
</launch>

‎arc_behaviour/src/NavigationAdapter.cpp‎

Lines changed: 20 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -4,36 +4,49 @@
44
#include "std_srvs/Empty.h"
55

66
using namespace arc_behaviour;
7+
bool NavigationAdapter::is_stuck_cb(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
8+
return this->is_stuck;
9+
}
10+
711

812
NavigationAdapter::NavigationAdapter() {
913
ros::NodeHandle nh;
10-
this->global_handle = nh;
1114
ros::NodeHandle local_handle("navigation_adapter");
15+
16+
this->global_handle = nh;
1217
this->local_handle = local_handle;
18+
this->is_stuck = false;
19+
1320
ROS_INFO("Setting up navigation adapter..");
21+
1422
this->move_to_goal_server = local_handle.advertiseService("move_to_goal",&NavigationAdapter::move_to_goal_request_cb, this);
23+
this->is_stuck_server = local_handle.advertiseService("is_stuck",&NavigationAdapter::is_stuck_cb, this);
1524
this->move_client = new MoveBaseClient("move_base", true);
25+
1626
//wait for the action server to come up
1727
while(!move_client->waitForServer(ros::Duration(5.0))) { //TODO: Better checking to ensure action server is actually up. If it's not, we need to let user know.
1828
ROS_INFO("Waiting for the move_base action server to come up");
1929
}
30+
2031
ROS_INFO("Found move_base action server!");
2132
this->current_nav_priority = 0;
2233
}
2334

2435
void NavigationAdapter::move_to_goal_result_cb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result) {
2536
if (state == actionlib::SimpleClientGoalState::ABORTED){
2637
ROS_INFO("Failed to move");
38+
//we just assume that if the robot couldn't move, and the goal was aborted because of this, we are stuck.
39+
this->is_stuck = true;
2740
}else if(state == actionlib::SimpleClientGoalState::SUCCEEDED){
2841
ROS_INFO("successfully moved to goal!!!");
42+
this->is_stuck = false; //not stuck if you just reached a goal.
2943
}
3044

3145
//either way, we are done navigating (until another request is sent)
3246
this->current_nav_priority = this->DEFAULT_NAVIGATION_PRIORITY;
3347
this->goal_active = false;
3448
}
3549
//TODO: Define a valid priority range, and ensure requests are within that range. If message is sent without priority specified, it wil be 412412 or something high, and will automatically go through...
36-
3750
bool NavigationAdapter::move_to_goal_request_cb(arc_msgs::NavigationRequest::Request &req, arc_msgs::NavigationRequest::Response &res) {
3851
ROS_INFO("Received request to move to goal (%d, %d) with priority %d", req.pose.position.x, req.pose.position.y, req.priority);
3952

@@ -42,7 +55,7 @@ bool NavigationAdapter::move_to_goal_request_cb(arc_msgs::NavigationRequest::Req
4255
assert(this->current_nav_priority >= this->DEFAULT_NAVIGATION_PRIORITY);
4356

4457
//Let higher priority request take control of navigation stack.
45-
if(req.priority > this->current_nav_priority) {
58+
if(req.priority >= this->current_nav_priority) {
4659
this->sendGoal(req);
4760
return true;
4861
} else {
@@ -75,17 +88,16 @@ void NavigationAdapter::sendGoal(arc_msgs::NavigationRequest::Request &req) {
7588
this->move_client->sendGoal(goal, boost::bind(&NavigationAdapter::move_to_goal_result_cb, this,_1,_2));
7689
this->current_nav_priority = req.priority;
7790
this->goal_active = true;
91+
this->is_stuck = false; //we aren't stuck at the moment we send a goal
7892
}
7993

8094
void NavigationAdapter::run() {
8195
ros::Rate r(10); //TODO: Make this a parameter, just get rid of magic numbers.
8296

8397
while(ros::ok()) {
84-
/*if(this->move_client.getState() == actionlib::SimpleClientGoalState::ACTIVE) {
85-
ROS_INFO("Navigation goal is active.");
86-
} else {
87-
ROS_INFO("navigation is not active");
88-
}*/
98+
if(this->is_stuck) {
99+
ROS_INFO("ROBOT IS STUCK");
100+
}
89101
ros::spinOnce();
90102
r.sleep();
91103
}

‎arc_behaviour/src/RandomWanderMS.cpp‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
#include "RandomWanderMS.h"
2+
#include "arc_msgs/NavigationRequest.h"
3+
4+
using namespace arc_behaviour;
5+
6+
RandomWanderMS::RandomWanderMS() {
7+
ros::NodeHandle global_handle;
8+
this->global_handle = global_handle;
9+
ros::NodeHandle local_handle("random_wander_ms");
10+
this->local_handle = local_handle;
11+
ROS_INFO("Setting up random wander ms");
12+
local_handle.param("max_range", this->max_range, this->DEFAULT_MAX_RANGE);
13+
local_handle.param("update_goal_feq", this->random_choice_rate, this->DEFAULT_RANDOM_CHOICE_RATE);
14+
this->move_to_goal_client = this->global_handle.serviceClient<arc_msgs::NavigationRequest>("navigation_adapter/move_to_goal");
15+
this->priority = local_handle.getParam("priority", this->DEFAULT_PRIORITY);
16+
ROS_INFO("Parameter max_range set: %f", this->max_range);
17+
ROS_INFO("Parameter update_goal_greq set: %f", this->random_choice_rate);
18+
}
19+
20+
void RandomWanderMS::setMaxRange(double max_range) {
21+
if(max_range<=0) {
22+
ROS_WARN("Unable to set parameter: max_range. Value must be > 0. Using default.");
23+
this->max_range = this->DEFAULT_MAX_RANGE;
24+
} else {
25+
this->max_range = max_range;
26+
}
27+
}
28+
29+
30+
void RandomWanderMS::run() {
31+
ros::Rate r(this->random_choice_rate);
32+
33+
while(ros::ok()) {
34+
arc_msgs::NavigationRequest req = this->generateRequest();
35+
this->move_to_goal_client.call(req);
36+
37+
ROS_INFO("sent random wander goal of (%d, %d)", (int)req.request.pose.position.x, (int)req.request.pose.position.y);
38+
39+
ros::spinOnce();
40+
r.sleep();
41+
}
42+
43+
}
44+
45+
arc_msgs::NavigationRequest RandomWanderMS::generateRequest() {
46+
arc_msgs::NavigationRequest req;
47+
assert(this->max_range>0);
48+
double x = (rand() % (int)this->max_range) - ((int)this->max_range)/2.0; //TODO: If maxrange is a double, this will truncate some accuracy. Fix the mod op to support double value for max_range
49+
double y = (rand() % (int)this->max_range) - ((int)this->max_range)/2.0; //TODO: Test this random generation, make sure it's proper.
50+
51+
ROS_INFO("Generated (%d, %d) ", (int)x, (int)y);
52+
req.request.priority = this->priority;
53+
req.request.pose.position.x = x;
54+
req.request.pose.position.y = y;
55+
req.request.pose.position.z = 0;
56+
req.request.pose.orientation.w = 1.0; //default
57+
req.request.pose.orientation.x = 0; //default
58+
req.request.pose.orientation.y = 0; //default
59+
req.request.pose.orientation.z = 0; //default
60+
61+
return req;
62+
}

‎arc_behaviour/src/RandomWanderMS.h‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
/**
2+
* CLASS: RandomWanderMS
3+
* DATE: 20/02/17
4+
* AUTHOR: Kyle Morris
5+
* DESCRIPTION: Random wander behaviour that sends various random navigation goals to navigation stack.
6+
*/
7+
#ifndef ARC_BEHAVIOUR_RANDOMWANDERMS_H
8+
#define ARC_BEHAVIOUR_RANDOMWANDERMS_H
9+
#include "ros/ros.h"
10+
#include "arc_msgs/NavigationRequest.h"
11+
12+
namespace arc_behaviour {
13+
class RandomWanderMS {
14+
private:
15+
double DEFAULT_RANDOM_CHOICE_RATE = 0.1; //update every 10 second TODO: Read this as parameter
16+
int DEFAULT_PRIORITY = 1;
17+
double DEFAULT_MAX_RANGE = 10.0;
18+
/*
19+
* Client to randomly send requests for navigation to navigation stack.
20+
*/
21+
ros::ServiceClient move_to_goal_client;
22+
23+
ros::NodeHandle global_handle;
24+
ros::NodeHandle local_handle;
25+
26+
/**
27+
* How often (in hz) we will update the random goal.
28+
*/
29+
double random_choice_rate;
30+
31+
/**
32+
* how much priority this behaviour has. Will determine if it is accepted by navigation adapter.
33+
*/
34+
int priority;
35+
36+
/**
37+
* Maximum distance in meters, that a goal can be. Relative to current location.
38+
*/
39+
double max_range;
40+
41+
/**
42+
* Generate a random navigation request
43+
* @return NavigationRequest generated.
44+
*/
45+
arc_msgs::NavigationRequest generateRequest();
46+
47+
void setMaxRange(double max_range);
48+
49+
50+
public:
51+
RandomWanderMS();
52+
53+
void run();
54+
};
55+
}
56+
57+
#endif //ARC_BEHAVIOUR_RANDOMWANDERMS_H

‎arc_behaviour/src/random_wander_ms_node.cpp‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
#include "ros/ros.h"
2+
#include "RandomWanderMS.h"
3+
4+
int main(int argc, char **argv) {
5+
ros::init(argc, argv, "random_wander_ms_node");
6+
arc_behaviour::RandomWanderMS random_wander;
7+
random_wander.run();
8+
return 0;
9+
}

‎arc_control/launch/default.launch‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,8 @@
33
<group ns="arc">
44
<group ns="test_bot">
55
<include file="$(find arc_behaviour)/launch/default.launch" />
6+
<include file="$(find arc_navigation)/launch/default.launch" />
67
<include file="$(find arc_stage)/launch/default.launch" />
7-
<include file="$(find arc_navigation)/launch/default.launch" />
88
</group>
99
</group>
1010
</launch>

‎arc_control/launch/record_1‎

11.1 MB
Binary file not shown.

‎arc_control/launch/testing.launch‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
<launch>
2+
<!-- All content of ARC will be contained in the /arc namespace -->
3+
<group ns="arc">
4+
<group ns="test_bot">
5+
<!-- <include file="$(find arc_behaviour)"/launch/default.launch" />-->
6+
<!-- <include file="$(find arc_stage)/launch/default.launch" /> -->
7+
<include file="$(find arc_navigation)/launch/default.launch" />
8+
</group>
9+
</group>
10+
</launch>

0 commit comments

Comments
 (0)