Skip to content
Snippets Groups Projects
Commit c6fbce3e authored by Timothy Wiley's avatar Timothy Wiley
Browse files

crosbot_explore: Basic control client can recieve target point or pose

parent e2e5e3c2
Branches
No related tags found
No related merge requests found
...@@ -6,11 +6,16 @@ project(crosbot_explore) ...@@ -6,11 +6,16 @@ project(crosbot_explore)
## is used, also find other catkin packages ## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
crosbot crosbot_map geometry_msgs
geometry_msgs nav_msgs nav_msgs
dynamic_reconfigure dynamic_reconfigure
message_generation message_generation
genmsg actionlib_msgs actionlib genmsg
actionlib_msgs
actionlib
crosbot
crosbot_map
crosbot_msgs
) )
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
...@@ -77,8 +82,13 @@ catkin_package( ...@@ -77,8 +82,13 @@ catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include
LIBRARIES crosbot_explore LIBRARIES crosbot_explore
CATKIN_DEPENDS CATKIN_DEPENDS
crosbot crosbot_map roscpp geometry_msgs nav_msgs roscpp
geometry_msgs
nav_msgs
actionlib_msgs actionlib_msgs
crosbot
crosbot_map
crosbot_msgs
# DEPENDS system_lib # DEPENDS system_lib
) )
......
...@@ -70,6 +70,12 @@ private: ...@@ -70,6 +70,12 @@ private:
*/ */
void setExploreMode(int mode); void setExploreMode(int mode);
/**
* Get command points
*/
crosbot::Pose3D getCommandPoint(const crosbot_msgs::ControlCommandPtr command);
crosbot::Pose3D getCommandPose(const crosbot_msgs::ControlCommandPtr command);
/** /**
* Get A* path to origin and set path * Get A* path to origin and set path
*/ */
......
...@@ -11,6 +11,8 @@ ...@@ -11,6 +11,8 @@
#define DEFAULT_CONTROL_NAMESPACE "crosbot_explore" #define DEFAULT_CONTROL_NAMESPACE "crosbot_explore"
#define COMMAND_GO_TO_ORIGIN "explore_command_go_to_origin" #define COMMAND_GO_TO_ORIGIN "explore_command_go_to_origin"
#define COMMAND_GO_TO_POINT "explore_command_go_to_point"
#define COMMAND_GO_TO_POSE "explore_command_go_to_pose"
#define COMMAND_LEFTWALL_FOLLOW "explore_command_leftwall_follow" #define COMMAND_LEFTWALL_FOLLOW "explore_command_leftwall_follow"
#define COMMAND_RIGHTWALL_FOLLOW "explore_command_rightwall_follow" #define COMMAND_RIGHTWALL_FOLLOW "explore_command_rightwall_follow"
......
...@@ -35,6 +35,7 @@ ...@@ -35,6 +35,7 @@
<depend>actionlib_msgs</depend> <depend>actionlib_msgs</depend>
<depend>crosbot</depend> <depend>crosbot</depend>
<depend>crosbot_map</depend> <depend>crosbot_map</depend>
<depend>crosbot_msgs</depend>
<depend>dynamic_reconfigure</depend> <depend>dynamic_reconfigure</depend>
<depend>geometry_msgs</depend> <depend>geometry_msgs</depend>
<depend>nav_msgs</depend> <depend>nav_msgs</depend>
......
...@@ -112,6 +112,20 @@ void BasicExplorerClient::callback_receivedCommand(const crosbot_msgs::ControlCo ...@@ -112,6 +112,20 @@ void BasicExplorerClient::callback_receivedCommand(const crosbot_msgs::ControlCo
setExploreMode(crosbot_explore::ExploreMode::MODE_RIGHT_WALL); setExploreMode(crosbot_explore::ExploreMode::MODE_RIGHT_WALL);
} else if (command->command == COMMAND_GO_TO_ORIGIN) { } else if (command->command == COMMAND_GO_TO_ORIGIN) {
setAStarPose(crosbot::Pose3D(), false); setAStarPose(crosbot::Pose3D(), false);
} else if (command->command == COMMAND_GO_TO_POINT) {
crosbot::Pose3D toPose = getCommandPoint(command);
if (toPose.isFinite()) {
setAStarPose(toPose, false);
} else {
STATUS_ERROR(crosbotStatus, "%s Go to Point failed - invalid point specified", LOG_START);
}
} else if (command->command == COMMAND_GO_TO_POSE) {
crosbot::Pose3D toPose = getCommandPose(command);
if (toPose.isFinite()) {
setAStarPose(toPose, true);
} else {
STATUS_ERROR(crosbotStatus, "%s Go to Pose failed - invalid point specified", LOG_START);
}
} }
} }
...@@ -137,6 +151,31 @@ void BasicExplorerClient::actionserver_feedback(const crosbot_explore::ExploreFe ...@@ -137,6 +151,31 @@ void BasicExplorerClient::actionserver_feedback(const crosbot_explore::ExploreFe
STATUS_INFO(crosbotStatus, "Explorer: %s (%s)", feedback->status.c_str(), searchStrategy.c_str()); STATUS_INFO(crosbotStatus, "Explorer: %s (%s)", feedback->status.c_str(), searchStrategy.c_str());
} }
crosbot::Pose3D BasicExplorerClient::getCommandPoint(const crosbot_msgs::ControlCommandPtr command) {
crosbot::Pose3D toPoint(INFINITY, INFINITY, INFINITY);
if (command->args_doubles.size() >= 3) {
toPoint.position.x = command->args_doubles[0];
toPoint.position.y = command->args_doubles[1];
toPoint.position.z = command->args_doubles[2];
}
return toPoint;
}
crosbot::Pose3D BasicExplorerClient::getCommandPose(const crosbot_msgs::ControlCommandPtr command) {
crosbot::Pose3D toPose = getCommandPoint(command);
if (command->args_doubles.size() >= 7) {
toPose.orientation.x = command->args_doubles[3];
toPose.orientation.y = command->args_doubles[4];
toPose.orientation.z = command->args_doubles[5];
toPose.orientation.w = command->args_doubles[6];
}
return toPose;
}
void BasicExplorerClient::setExploreMode(int mode) { void BasicExplorerClient::setExploreMode(int mode) {
// Ensure action server has started // Ensure action server has started
bool hasServer = actionClient->waitForServer(ros::Duration(DEFAULT_SERVER_WAIT)); bool hasServer = actionClient->waitForServer(ros::Duration(DEFAULT_SERVER_WAIT));
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment