Add option to create publishers latched with parameters

include/actionlib/client/action_client.h

@@ -225,6 +225,9 @@ class ActionClient
     if (pub_queue_size < 0) {pub_queue_size = 10;}
     if (sub_queue_size < 0) {sub_queue_size = 1;}
 
+    bool pub_latch;
+    n_.param("actionlib_client_pub_latch", pub_latch, false);
+
     status_sub_ = queue_subscribe("status", static_cast<uint32_t>(sub_queue_size),
         &ActionClientT::statusCb, this, queue);
     feedback_sub_ = queue_subscribe("feedback", static_cast<uint32_t>(sub_queue_size),
@@ -238,12 +241,12 @@ class ActionClient
     goal_pub_ = queue_advertise<ActionGoal>("goal", static_cast<uint32_t>(pub_queue_size),
         boost::bind(&ConnectionMonitor::goalConnectCallback, connection_monitor_, _1),
         boost::bind(&ConnectionMonitor::goalDisconnectCallback, connection_monitor_, _1),
-        queue);
+        queue, pub_latch);
     cancel_pub_ =
       queue_advertise<actionlib_msgs::GoalID>("cancel", static_cast<uint32_t>(pub_queue_size),
         boost::bind(&ConnectionMonitor::cancelConnectCallback, connection_monitor_, _1),
         boost::bind(&ConnectionMonitor::cancelDisconnectCallback, connection_monitor_, _1),
-        queue);
+        queue, pub_latch);
 
     manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
     manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
@@ -253,12 +256,13 @@ class ActionClient
   ros::Publisher queue_advertise(const std::string & topic, uint32_t queue_size,
     const ros::SubscriberStatusCallback & connect_cb,
     const ros::SubscriberStatusCallback & disconnect_cb,
-    ros::CallbackQueueInterface * queue)
+    ros::CallbackQueueInterface * queue,
+    bool latch)
   {
     ros::AdvertiseOptions ops;
     ops.init<M>(topic, queue_size, connect_cb, disconnect_cb);
     ops.tracked_object = ros::VoidPtr();
-    ops.latch = false;
+    ops.latch = latch;
     ops.callback_queue = queue;
     return n_.advertise(ops);
   }

include/actionlib/server/action_server_imp.h

@@ -144,7 +144,11 @@ void ActionServer<ActionSpec>::initialize()
   if (pub_queue_size < 0) {pub_queue_size = 50;}
   if (sub_queue_size < 0) {sub_queue_size = 50;}
 
-  result_pub_ = node_.advertise<ActionResult>("result", static_cast<uint32_t>(pub_queue_size));
+  bool latch_result_pub;
+  node_.param("actionlib_server_result_latched", latch_result_pub, false);
+
+  result_pub_ = node_.advertise<ActionResult>("result", static_cast<uint32_t>(pub_queue_size),
+    latch_result_pub);
   feedback_pub_ =
     node_.advertise<ActionFeedback>("feedback", static_cast<uint32_t>(pub_queue_size));
   status_pub_ =

test/CMakeLists.txt

@@ -17,6 +17,9 @@ if(GTEST_FOUND)
   add_executable(actionlib-server_goal_handle_destruction EXCLUDE_FROM_ALL server_goal_handle_destruction.cpp)
   target_link_libraries(actionlib-server_goal_handle_destruction ${PROJECT_NAME} ${GTEST_LIBRARIES})
 
+  add_executable(actionlib-simple_client_latched_test EXCLUDE_FROM_ALL simple_client_latched_test.cpp)
+  target_link_libraries(actionlib-simple_client_latched_test ${PROJECT_NAME} ${GTEST_LIBRARIES})
+
   add_executable(actionlib-simple_client_wait_test EXCLUDE_FROM_ALL simple_client_wait_test.cpp)
   target_link_libraries(actionlib-simple_client_wait_test ${PROJECT_NAME} ${GTEST_LIBRARIES})
 
@@ -41,6 +44,7 @@ if(GTEST_FOUND)
       actionlib-server_goal_handle_destruction
       actionlib-simple_client_wait_test
       actionlib-simple_client_allocator_test
+      actionlib-simple_client_latched_test
       actionlib-action_client_destruction_test
       actionlib-test_cpp_simple_client_cancel_crash
       actionlib-exercise_simple_client
@@ -55,6 +59,7 @@ add_rostest(test_cpp_simple_client_allocator.launch)
 add_rostest(test_cpp_action_client_destruction.launch)
 add_rostest(test_server_goal_handle_destruction.launch)
 add_rostest(test_cpp_simple_client_cancel_crash.launch)
+add_rostest(test_cpp_simple_client_latched.launch)
 add_rostest(test_imports.launch)
 add_rostest(test_python_server_components.launch)
 add_rostest(test_python_server.launch)

test/simple_client_latched_test.cpp

@@ -0,0 +1,84 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include <gtest/gtest.h>
+#include <actionlib/TestAction.h>
+#include <actionlib/client/simple_action_client.h>
+
+using namespace actionlib;
+
+TEST(SimpleClient, easy_latched_test) {
+  ros::NodeHandle n; //("latched_client");
+  n.setParam("reference_action/actionlib_client_pub_latch", true);
+
+  SimpleActionClient<TestAction> client(n, "reference_action");
+
+  TestGoal goal;
+  SimpleClientGoalState state(SimpleClientGoalState::LOST);
+
+  goal.goal = 1;
+  state = client.sendGoalAndWait(goal, ros::Duration(10, 0), ros::Duration(10, 0));
+  EXPECT_TRUE(state == SimpleClientGoalState::SUCCEEDED) <<
+    "Expected [SUCCEEDED], but goal state is [" << client.getState().toString() << "]";
+
+  goal.goal = 4;
+  state = client.sendGoalAndWait(goal, ros::Duration(2, 0), ros::Duration(1, 0));
+  EXPECT_TRUE(state == SimpleClientGoalState::PREEMPTED) <<
+    "Expected [PREEMPTED], but goal state is [" << client.getState().toString() << "]";
+}
+
+void spinThread()
+{
+  ros::NodeHandle nh;
+  ros::spin();
+}
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+
+  ros::init(argc, argv, "simple_client_test");
+
+  boost::thread spin_thread(&spinThread);
+
+  int result = RUN_ALL_TESTS();
+
+  ros::shutdown();
+
+  spin_thread.join();
+
+  return result;
+}

test/simple_execute_ref_server.cpp

@@ -34,6 +34,8 @@
 
 //! \author Vijay Pradeep
 
+#include <sstream>
+
 #include <actionlib/server/simple_action_server.h>
 #include <actionlib/TestAction.h>
 #include <ros/ros.h>
@@ -100,6 +102,18 @@ void SimpleExecuteRefServer::executeCallback(const TestGoalConstPtr & goal)
 int main(int argc, char ** argv)
 {
   ros::init(argc, argv, "ref_server");
+  ros::start();
+
+  std::vector<std::string> args;
+  ros::removeROSArgs(argc, argv, args);
+
+  if(args.size() > 1 && "--delay" == args[1]) {
+    std::stringstream stringValue;
+    stringValue << args[2];
+    unsigned int delay = 0;
+    stringValue >> delay;
+    ros::Duration(delay).sleep();
+  }
 
   SimpleExecuteRefServer server;
 

test/test_cpp_simple_client_latched.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="actionlib" type="actionlib-simple_execute_ref_server" name="ref_server" output="screen" args="--delay 5"/>
+
+  <test test-name="simple_client_latched_test" pkg="actionlib" type="actionlib-simple_client_latched_test"/>
+</launch>