Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20228] pkg/trunk
|
From: <vij...@us...> - 2009-07-31 01:02:30
|
Revision: 20228
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20228&view=rev
Author: vijaypradeep
Date: 2009-07-31 01:02:21 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
Working on simple action client. Moving move_base client code to a new package
Modified Paths:
--------------
pkg/trunk/sandbox/actionlib/CMakeLists.txt
pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp
pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h
pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h
pkg/trunk/stacks/navigation/move_base/manifest.xml
Added Paths:
-----------
pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_goal_state.h
pkg/trunk/sandbox/move_base_client/
pkg/trunk/sandbox/move_base_client/CMakeLists.txt
pkg/trunk/sandbox/move_base_client/Makefile
pkg/trunk/sandbox/move_base_client/mainpage.dox
pkg/trunk/sandbox/move_base_client/manifest.xml
pkg/trunk/sandbox/move_base_client/src/
pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp
Removed Paths:
-------------
pkg/trunk/sandbox/actionlib/src/move_base_client.cpp
Modified: pkg/trunk/sandbox/actionlib/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/actionlib/CMakeLists.txt 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/CMakeLists.txt 2009-07-31 01:02:21 UTC (rev 20228)
@@ -22,7 +22,8 @@
#rospack_add_executable(goal_manager_test src/goal_manager_test.cpp)
rospack_add_executable(move_base_server src/move_base_server.cpp)
-rospack_add_executable(move_base_client src/move_base_client.cpp)
+#rospack_add_executable(move_base_client src/move_base_client.cpp)
+#rospack_add_executable(simple_move_base_client src/simple_move_base_client.cpp)
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/action_client.h 2009-07-31 01:02:21 UTC (rev 20228)
@@ -111,12 +111,11 @@
*/
void cancelAllGoals()
{
- ActionGoal cancel_msg;
+ GoalID cancel_msg;
// CancelAll policy encoded by stamp=0, id=0
- cancel_msg.goal_id.stamp = ros::Time(0,0);
- cancel_msg.goal_id.id = ros::Time(0,0);
- cancel_msg.request_type.type = RequestType::PREEMPT_REQUEST;
- goal_pub_.publish(cancel_msg);
+ cancel_msg.stamp = ros::Time(0,0);
+ cancel_msg.id = ros::Time(0,0);
+ cancel_pub_.publish(cancel_msg);
}
/**
@@ -137,6 +136,7 @@
ros::Subscriber feedback_sub_;
ros::Publisher goal_pub_;
+ ros::Publisher cancel_pub_;
ros::Subscriber status_sub_;
ros::Subscriber result_sub_;
@@ -147,11 +147,18 @@
goal_pub_.publish(action_goal);
}
+ void sendCancelFunc(const GoalID& cancel_msg)
+ {
+ cancel_pub_.publish(cancel_msg);
+ }
+
void initClient()
{
// Start publishers and subscribers
goal_pub_ = n_.advertise<ActionGoal>("goal", 1);
+ cancel_pub_ = n_.advertise<GoalID>("cancel", 1);
manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
+ manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
status_sub_ = n_.subscribe("status", 1, &ActionClientT::statusCb, this);
feedback_sub_ = n_.subscribe("feedback", 1, &ActionClientT::feedbackCb, this);
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_handle.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -154,13 +154,12 @@
ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();
- boost::shared_ptr<ActionGoal> cancel_msg(new ActionGoal);
- cancel_msg->goal_id.stamp = ros::Time();
- cancel_msg->goal_id.id = action_goal->goal_id.id;
- cancel_msg->request_type.type = RequestType::PREEMPT_REQUEST;
+ GoalID cancel_msg;
+ cancel_msg.stamp = ros::Time(0,0);
+ cancel_msg.id = list_handle_.getElem()->getActionGoal()->goal_id.id;
- if (gm_->send_goal_func_)
- gm_->send_goal_func_(cancel_msg);
+ if (gm_->cancel_func_)
+ gm_->cancel_func_(cancel_msg);
list_handle_.getElem()->transitionToState(*this, CommState::WAITING_FOR_CANCEL_ACK);
}
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -46,6 +46,13 @@
}
template<class ActionSpec>
+void GoalManager<ActionSpec>::registerCancelFunc(CancelFunc cancel_func)
+{
+ cancel_func_ = cancel_func;
+}
+
+
+template<class ActionSpec>
GoalHandle<ActionSpec> GoalManager<ActionSpec>::initGoal(const Goal& goal,
TransitionCallback transition_cb,
FeedbackCallback feedback_cb )
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/goal_manager.h 2009-07-31 01:02:21 UTC (rev 20228)
@@ -83,10 +83,13 @@
typedef boost::function<void (GoalHandle<ActionSpec>) > TransitionCallback;
typedef boost::function<void (GoalHandle<ActionSpec>, const FeedbackConstPtr&) > FeedbackCallback;
typedef boost::function<void (const ActionGoalConstPtr)> SendGoalFunc;
+ typedef boost::function<void (const GoalID&)> CancelFunc;
GoalManager() { }
void registerSendGoalFunc(SendGoalFunc send_goal_func);
+ void registerCancelFunc(CancelFunc cancel_func);
+
GoalHandle<ActionSpec> initGoal( const Goal& goal,
TransitionCallback transition_cb = TransitionCallback(),
FeedbackCallback feedback_cb = FeedbackCallback() );
@@ -102,6 +105,8 @@
ManagedListT list_;
private:
SendGoalFunc send_goal_func_ ;
+ CancelFunc cancel_func_ ;
+
boost::recursive_mutex list_mutex_;
GoalIDGenerator id_generator_;
Modified: pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_action_client.h 2009-07-31 01:02:21 UTC (rev 20228)
@@ -35,6 +35,9 @@
#ifndef ACTIONLIB_SIMPLE_ACTION_CLIENT_H_
#define ACTIONLIB_SIMPLE_ACTION_CLIENT_H_
+#include <boost/thread/condition.hpp>
+#include <boost/thread/mutex.hpp>
+
#include "ros/ros.h"
#include "actionlib/client/action_client.h"
#include "actionlib/client/simple_goal_state.h"
@@ -69,7 +72,7 @@
* Constructs a SingleGoalActionClient and sets up the necessary ros topics for the ActionInterface
* \param name The action name. Defines the namespace in which the action communicates
*/
- SimpleActionClient(const std::string& name) : ac_(name)
+ SimpleActionClient(const std::string& name) : ac_(name), cur_simple_state_(SimpleGoalState::PENDING)
{
initSimpleClient();
}
@@ -82,7 +85,7 @@
* \param n The node handle on top of which we want to namespace our action
* \param name The action name. Defines the namespace in which the action communicates
*/
- SimpleActionClient(const ros::NodeHandle& n, const std::string& name) : ac_(n, name)
+ SimpleActionClient(const ros::NodeHandle& n, const std::string& name) : ac_(n, name), cur_simple_state_(SimpleGoalState::PENDING)
{
initSimpleClient();
}
@@ -103,6 +106,7 @@
/**
* \brief Blocks until this goal transitions to done
+ * Note that this call exits only after the SimpleDoneCallback() is called.
*/
void waitForGoalToFinish();
@@ -154,6 +158,10 @@
SimpleGoalState cur_simple_state_;
+ // Signalling Stuff
+ boost::condition done_condition_;
+ boost::mutex done_mutex_;
+
// User Callbacks
SimpleDoneCallback done_cb_;
SimpleActiveCallback active_cb_;
@@ -163,6 +171,8 @@
void initSimpleClient();
void handleTransition(GoalHandleT gh);
void handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback);
+ void setSimpleState(const SimpleGoalState::StateEnum& next_state);
+ void setSimpleState(const SimpleGoalState& next_state);
};
@@ -174,6 +184,21 @@
}
template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState::StateEnum& next_state)
+{
+ setSimpleState( SimpleGoalState(next_state) );
+}
+
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::setSimpleState(const SimpleGoalState& next_state)
+{
+ ROS_DEBUG("Transitioning SimpleState from [%s] to [%s]",
+ cur_simple_state_.toString().c_str(),
+ next_state.toString().c_str());
+ cur_simple_state_ = next_state;
+}
+
+template<class ActionSpec>
void SimpleActionClient<ActionSpec>::sendGoal(const Goal& goal,
SimpleDoneCallback done_cb,
SimpleActiveCallback active_cb,
@@ -187,6 +212,8 @@
active_cb_ = active_cb;
feedback_cb_ = feedback_cb;
+ cur_simple_state_ = SimpleGoalState::PENDING;
+
// Send the goal to the ActionServer
gh_ = ac_.sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
boost::bind(&SimpleActionClientT::handleFeedback, this, _1));
@@ -254,19 +281,127 @@
gh_.cancel();
}
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::stopTrackingGoal()
+{
+ if (!gh_.isActive())
+ ROS_ERROR("Trying to stopTrackingGoal() when no goal is running. You are incorrectly using SimpleActionClient");
+ gh_.reset();
+}
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::handleFeedback(GoalHandleT gh, const FeedbackConstPtr& feedback)
+{
+ if (gh_ != gh)
+ ROS_ERROR("Got a callback on a goalHandle that we're not tracking. \
+ This is an internal SimpleActionClient/ActionClient bug. \
+ This could also be a GoalID collision");
+ if (feedback_cb_)
+ feedback_cb_(feedback);
+}
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::handleTransition(GoalHandleT gh)
+{
+ CommState comm_state_ = gh.getCommState();
+ switch (comm_state_.state_)
+ {
+ case CommState::WAITING_FOR_GOAL_ACK:
+ ROS_ERROR("BUG: Shouldn't ever get a transition callback for WAITING_FOR_GOAL_ACK");
+ break;
+ case CommState::PENDING:
+ ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
+ "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
+ comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
+ break;
+ case CommState::ACTIVE:
+ switch (cur_simple_state_.state_)
+ {
+ case SimpleGoalState::PENDING:
+ setSimpleState(SimpleGoalState::ACTIVE);
+ if (active_cb_)
+ active_cb_();
+ break;
+ case SimpleGoalState::ACTIVE:
+ break;
+ case SimpleGoalState::DONE:
+ ROS_ERROR("BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
+ comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
+ break;
+ default:
+ ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
+ break;
+ }
+ break;
+ case CommState::WAITING_FOR_RESULT:
+ break;
+ case CommState::WAITING_FOR_CANCEL_ACK:
+ break;
+ case CommState::RECALLING:
+ ROS_ERROR_COND( cur_simple_state_ != SimpleGoalState::PENDING,
+ "BUG: Got a transition to CommState [%s] when our in SimpleGoalState [%s]",
+ comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
+ break;
+ case CommState::PREEMPTING:
+ switch (cur_simple_state_.state_)
+ {
+ case SimpleGoalState::PENDING:
+ setSimpleState(SimpleGoalState::ACTIVE);
+ if (active_cb_)
+ active_cb_();
+ break;
+ case SimpleGoalState::ACTIVE:
+ break;
+ case SimpleGoalState::DONE:
+ ROS_ERROR("BUG: Got a transition to CommState [%s] when in SimpleGoalState [%s]",
+ comm_state_.toString().c_str(), cur_simple_state_.toString().c_str());
+ break;
+ default:
+ ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
+ break;
+ }
+ break;
+ case CommState::DONE:
+ switch (cur_simple_state_.state_)
+ {
+ case SimpleGoalState::PENDING:
+ case SimpleGoalState::ACTIVE:
+ done_mutex_.lock();
+ setSimpleState(SimpleGoalState::DONE);
+ done_mutex_.unlock();
+ if (done_cb_)
+ done_cb_(gh.getTerminalState(), gh.getResult());
+ done_condition_.notify_all();
+ break;
+ case SimpleGoalState::DONE:
+ ROS_ERROR("BUG: Got a second transition to DONE");
+ break;
+ default:
+ ROS_FATAL("Unknown SimpleGoalState %u", cur_simple_state_.state_);
+ break;
+ }
+ break;
+ default:
+ ROS_ERROR("Unknown CommState received [%u]", comm_state_.state_);
+ break;
+ }
+}
+template<class ActionSpec>
+void SimpleActionClient<ActionSpec>::waitForGoalToFinish()
+{
+ if (!gh_.isActive())
+ ROS_ERROR("Trying to waitForGoalToFinish() when no goal is running. You are incorrectly using SimpleActionClient");
+ boost::mutex::scoped_lock lock(done_mutex_);
+ if (cur_simple_state_ != SimpleGoalState::DONE)
+ done_condition_.wait(lock);
+}
+}
-
-
-
-
-
#endif // ACTIONLIB_SINGLE_GOAL_ACTION_CLIENT_H_
Copied: pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_goal_state.h (from rev 20225, pkg/trunk/sandbox/actionlib/src/move_base_client.cpp)
===================================================================
--- pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_goal_state.h (rev 0)
+++ pkg/trunk/sandbox/actionlib/include/actionlib/client/simple_goal_state.h 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,107 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef ACTIONLIB_CLIENT_SIMPLE_GOAL_STATE_H_
+#define ACTIONLIB_CLIENT_SIMPLE_GOAL_STATE_H_
+
+#include <string>
+#include "ros/console.h"
+
+namespace actionlib
+{
+
+/**
+ * \brief Thin wrapper around an enum in order providing a simplified version of the
+ * communication state, but with less states than CommState
+ **/
+class SimpleGoalState
+{
+public:
+
+ //! \brief Defines the various states the SimpleGoalState can be in
+ enum StateEnum
+ {
+ PENDING,
+ ACTIVE,
+ DONE
+ } ;
+
+ SimpleGoalState(const StateEnum& state) : state_(state) { }
+
+ inline bool operator==(const SimpleGoalState& rhs) const
+ {
+ return (state_ == rhs.state_) ;
+ }
+
+ inline bool operator==(const SimpleGoalState::StateEnum& rhs) const
+ {
+ return (state_ == rhs);
+ }
+
+ inline bool operator!=(const SimpleGoalState::StateEnum& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ inline bool operator!=(const SimpleGoalState& rhs) const
+ {
+ return !(*this == rhs);
+ }
+
+ std::string toString() const
+ {
+ switch(state_)
+ {
+ case PENDING:
+ return "PENDING";
+ case ACTIVE:
+ return "ACTIVE";
+ case DONE:
+ return "DONE";
+ default:
+ ROS_ERROR("BUG: Unhandled SimpleGoalState: %u", state_);
+ break;
+ }
+ return "BUG-UNKNOWN";
+ }
+
+ StateEnum state_;
+private:
+ SimpleGoalState();
+
+} ;
+
+}
+
+#endif
Deleted: pkg/trunk/sandbox/actionlib/src/move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/actionlib/src/move_base_client.cpp 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/sandbox/actionlib/src/move_base_client.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -1,112 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-#include "ros/ros.h"
-
-#include "actionlib/MoveBaseAction.h"
-#include "actionlib/client/action_client.h"
-
-#include "boost/thread.hpp"
-
-using namespace actionlib;
-using namespace robot_msgs;
-
-typedef ActionClient<MoveBaseAction> MoveBaseClient;
-
-void goalCallback(GoalHandle<MoveBaseAction> gh)
-{
- ROS_DEBUG("In the goalCallback");
-
- ROS_DEBUG("Our final status is: [%s]", gh.getCommState().toString().c_str());
-
- if (gh.getResult())
- ROS_DEBUG("Got a Result!");
- else
- ROS_DEBUG("Got a NULL Result.");
-
-}
-
-void feedbackCallback(GoalHandle<MoveBaseAction> gh, const MoveBaseFeedbackConstPtr& fb)
-{
- ROS_INFO("Got Feedback!");
-}
-
-void spinThread()
-{
- ros::spin();
-}
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "move_base_action_client");
-
- ros::NodeHandle n;
-
- boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
-
- MoveBaseClient ac("move_base");
-
- //ros::Duration sleep_duration = ros::Duration().fromSec(1.0);
- //sleep_duration.sleep();
- sleep(2.0);
-
- MoveBaseGoal goal;
-
- GoalHandle<MoveBaseAction> gh = ac.sendGoal(goal, &goalCallback, &feedbackCallback);
-
- sleep(1.0);
-
- gh.cancel();
-
- /*sleep(2.0);
-
- ROS_INFO("About to send a goal");
- gh.reset();
- gh = ac.sendGoal(goal, &goalCallback, &feedbackCallback, ros::Duration(10.0));
-
- sleep(2.0);
-
- ROS_INFO("About to preempt the goal");
- gh.preemptGoal();*/
-
- //ac.sendGoal(goal);
-
- while(n.ok())
- sleep(.1);
-
- sleep(3);
-
-
- return 0;
-}
Copied: pkg/trunk/sandbox/move_base_client/CMakeLists.txt (from rev 20225, pkg/trunk/sandbox/actionlib/CMakeLists.txt)
===================================================================
--- pkg/trunk/sandbox/move_base_client/CMakeLists.txt (rev 0)
+++ pkg/trunk/sandbox/move_base_client/CMakeLists.txt 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rospack(move_base_client)
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#genmsg()
+#uncomment if you have defined services
+#gensrv()
+
+rospack_add_executable(move_base_client src/move_base_client.cpp)
+rospack_add_executable(simple_move_base_client src/simple_move_base_client.cpp)
+
+#common commands for building c++ executables and libraries
+#rospack_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rospack_add_boost_directories()
+#rospack_link_boost(${PROJECT_NAME} thread)
+#rospack_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Added: pkg/trunk/sandbox/move_base_client/Makefile
===================================================================
--- pkg/trunk/sandbox/move_base_client/Makefile (rev 0)
+++ pkg/trunk/sandbox/move_base_client/Makefile 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/sandbox/move_base_client/mainpage.dox
===================================================================
--- pkg/trunk/sandbox/move_base_client/mainpage.dox (rev 0)
+++ pkg/trunk/sandbox/move_base_client/mainpage.dox 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b move_base_client is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/sandbox/move_base_client/manifest.xml
===================================================================
--- pkg/trunk/sandbox/move_base_client/manifest.xml (rev 0)
+++ pkg/trunk/sandbox/move_base_client/manifest.xml 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,15 @@
+<package>
+ <description brief="move_base_client">
+ move_base_client
+ </description>
+ <author>Vijay Pradeep</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/move_base_client</url>
+
+ <depend package="move_base" />
+ <depend package="actionlib" />
+
+</package>
+
+
Copied: pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp (from rev 20225, pkg/trunk/sandbox/actionlib/src/move_base_client.cpp)
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp (rev 0)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,112 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "ros/ros.h"
+
+#include "move_base/MoveBaseAction.h"
+#include "actionlib/client/action_client.h"
+
+#include "boost/thread.hpp"
+
+using namespace actionlib;
+using namespace robot_msgs;
+using namespace move_base;
+
+typedef ActionClient<MoveBaseAction> MoveBaseClient;
+
+void transitionCallback(GoalHandle<MoveBaseAction> gh)
+{
+ ROS_DEBUG("In the transition");
+
+ ROS_DEBUG("We have transitioned to: [%s]", gh.getCommState().toString().c_str());
+
+ if (gh.getResult())
+ ROS_DEBUG("Got a Result!");
+ else
+ ROS_DEBUG("NULL Result");
+}
+
+void feedbackCallback(GoalHandle<MoveBaseAction> gh, const MoveBaseFeedbackConstPtr& fb)
+{
+ ROS_INFO("Got Feedback!");
+}
+
+void spinThread()
+{
+ ros::spin();
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "move_base_action_client");
+
+ ros::NodeHandle n;
+
+ boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
+
+ MoveBaseClient ac("move_base");
+
+ //ros::Duration sleep_duration = ros::Duration().fromSec(1.0);
+ //sleep_duration.sleep();
+ sleep(2.0);
+
+ MoveBaseGoal goal;
+
+ GoalHandle<MoveBaseAction> gh = ac.sendGoal(goal, &transitionCallback, &feedbackCallback);
+
+ sleep(1.0);
+
+ gh.cancel();
+
+ /*sleep(2.0);
+
+ ROS_INFO("About to send a goal");
+ gh.reset();
+ gh = ac.sendGoal(goal, &goalCallback, &feedbackCallback, ros::Duration(10.0));
+
+ sleep(2.0);
+
+ ROS_INFO("About to preempt the goal");
+ gh.preemptGoal();*/
+
+ //ac.sendGoal(goal);
+
+ while(n.ok())
+ sleep(.1);
+
+ sleep(3);
+
+
+ return 0;
+}
Copied: pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp (from rev 20225, pkg/trunk/sandbox/actionlib/src/move_base_client.cpp)
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp (rev 0)
+++ pkg/trunk/sandbox/move_base_client/src/simple_move_base_client.cpp 2009-07-31 01:02:21 UTC (rev 20228)
@@ -0,0 +1,108 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "ros/ros.h"
+
+#include "move_base/MoveBaseAction.h"
+#include "actionlib/client/simple_action_client.h"
+
+#include "boost/thread.hpp"
+
+using namespace actionlib;
+using namespace robot_msgs;
+using namespace move_base;
+
+typedef SimpleActionClient<MoveBaseAction> MoveBaseClient;
+
+void activeCallback()
+{
+ ROS_DEBUG("In the activeCallback");
+}
+
+void doneCallback(const TerminalState& terminal_state)
+{
+ ROS_DEBUG("In the done callback with terminal state=[%s]", terminal_state.toString().c_str());
+}
+
+void feedbackCallback(const MoveBaseFeedbackConstPtr& fb)
+{
+ ROS_INFO("Got Feedback!");
+}
+
+void spinThread()
+{
+ ros::spin();
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "move_base_action_client");
+
+ ros::NodeHandle n;
+
+ boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
+
+ MoveBaseClient ac("move_base");
+
+ sleep(2.0);
+
+ MoveBaseGoal goal;
+
+ //ac.sendGoal(goal, &doneCallback, &activeCallback, &feedbackCallback);
+
+ sleep(1.0);
+
+ //ac.cancel();
+
+ /*sleep(2.0);
+
+ ROS_INFO("About to send a goal");
+ gh.reset();
+ gh = ac.sendGoal(goal, &goalCallback, &feedbackCallback, ros::Duration(10.0));
+
+ sleep(2.0);
+
+ ROS_INFO("About to preempt the goal");
+ gh.preemptGoal();*/
+
+ //ac.sendGoal(goal);
+
+ while(n.ok())
+ sleep(.1);
+
+ sleep(3);
+
+
+ return 0;
+}
Modified: pkg/trunk/stacks/navigation/move_base/manifest.xml
===================================================================
--- pkg/trunk/stacks/navigation/move_base/manifest.xml 2009-07-31 00:58:59 UTC (rev 20227)
+++ pkg/trunk/stacks/navigation/move_base/manifest.xml 2009-07-31 01:02:21 UTC (rev 20228)
@@ -28,6 +28,6 @@
<depend package="carrot_planner"/>
<export>
- <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lmove_base"/>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lmove_base"/>
</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
