Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21496] pkg/trunk
|
From: <vij...@us...> - 2009-08-11 01:30:24
|
Revision: 21496
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21496&view=rev
Author: vijaypradeep
Date: 2009-08-11 01:30:07 +0000 (Tue, 11 Aug 2009)
Log Message:
-----------
Building basic action for capturing gripper LED location in cameras
Modified Paths:
--------------
pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/robot_pixels_capture.cpp
pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h
pkg/trunk/vision/led_detection/stereo_left_detect.launch
pkg/trunk/vision/led_detection/stereo_right_detect.launch
Added Paths:
-----------
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_action_server.cpp
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_sample_client.cpp
Modified: pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt 2009-08-11 01:23:19 UTC (rev 21495)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt 2009-08-11 01:30:07 UTC (rev 21496)
@@ -24,7 +24,12 @@
rospack_add_executable(run_robot_pixels_capture src/run_robot_pixels_capture.cpp)
target_link_libraries(run_robot_pixels_capture ${PROJECT_NAME})
+rospack_add_executable(capture_robot_pixels_action_server src/capture_robot_pixels_action_server.cpp)
+target_link_libraries(capture_robot_pixels_action_server ${PROJECT_NAME})
+rospack_add_executable(capture_robot_pixels_sample_client src/capture_robot_pixels_sample_client.cpp)
+target_link_libraries(capture_robot_pixels_sample_client ${PROJECT_NAME})
+
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-add_subdirectory(test EXCLUDE_FROM_ALL)
\ No newline at end of file
+add_subdirectory(test EXCLUDE_FROM_ALL)
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_action_server.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_action_server.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_action_server.cpp 2009-08-11 01:30:07 UTC (rev 21496)
@@ -0,0 +1,114 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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/server/single_goal_action_server.h>
+#include <pr2_calibration_actions/robot_pixels_capture.h>
+#include <pr2_calibration_actions/CaptureRobotPixelsAction.h>
+
+using namespace pr2_calibration_actions;
+
+class CaptureRobotPixelsActionServer
+{
+public:
+ CaptureRobotPixelsActionServer(ros::NodeHandle nh, std::string action_name) :
+ as_(nh, action_name)
+ {
+ as_.registerPreemptCallback( boost::bind(&CaptureRobotPixelsActionServer::preemptCallback, this));
+ as_.registerGoalCallback( boost::bind(&CaptureRobotPixelsActionServer::goalCallback, this));
+ }
+
+ void preemptCallback()
+ {
+ boost::mutex::scoped_lock lock(capture_mutex_);
+ ROS_FATAL_COND(!capture_, "Trying to preempt a null capture_");
+ capture_->shutdown();
+ as_.setPreempted();
+ }
+
+ void goalCallback()
+ {
+ ROS_INFO("Got a Goal. Going to start listening to data");
+ boost::mutex::scoped_lock lock(capture_mutex_);
+ if ( as_.isActive() )
+ {
+ ROS_FATAL_COND(!capture_, "Trying to shutdown a null capture_");
+ capture_->shutdown();
+ as_.setPreempted();
+ }
+ capture_.reset();
+
+ cur_goal_ = as_.acceptNewGoal();
+
+ capture_.reset(new RobotPixelsCapture(cur_goal_->config,
+ boost::bind(&CaptureRobotPixelsActionServer::completionCallback, this, _1),
+ boost::bind(&CaptureRobotPixelsActionServer::sendFeedback, this, _1)));
+ }
+
+ void completionCallback(const RobotPixelsResult& result)
+ {
+ capture_->shutdown();
+ CaptureRobotPixelsResult full_result;
+ full_result.result = result;
+ as_.setSucceeded(full_result);
+ }
+
+ void sendFeedback(const RobotPixelsFeedback& feedback)
+ {
+ CaptureRobotPixelsFeedback full_feedback;
+ full_feedback.feedback = feedback;
+ //as_.publishFeedback(full_feedback);
+ }
+
+
+private:
+ actionlib::SingleGoalActionServer<pr2_calibration_actions::CaptureRobotPixelsAction> as_;
+
+ boost::mutex capture_mutex_;
+ boost::shared_ptr<RobotPixelsCapture> capture_;
+ CaptureRobotPixelsGoalConstPtr cur_goal_;
+
+};
+
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "capture_robot_pixels_action_node");
+
+ ros::NodeHandle nh;
+
+ CaptureRobotPixelsActionServer act(nh, "capture_robot_pixels");
+
+ ros::spin();
+}
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_sample_client.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_sample_client.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/capture_robot_pixels_sample_client.cpp 2009-08-11 01:30:07 UTC (rev 21496)
@@ -0,0 +1,153 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, 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/client/simple_action_client.h>
+#include <pr2_calibration_actions/CaptureRobotPixelsAction.h>
+#include <pr2_calibration_actions/robot_pixels_capture.h>
+
+using namespace pr2_calibration_actions;
+
+
+RobotPixelsConfig buildCommand()
+{
+ RobotPixelsConfig config;
+
+ config.joint_states_config.joint_states_topic = "joint_states";
+ config.joint_states_config.joint_names.resize(9);
+ config.joint_states_config.joint_names[0] = "head_pan_joint";
+ config.joint_states_config.joint_names[1] = "head_tilt_joint";
+ config.joint_states_config.joint_names[2] = "r_shoulder_pan_joint";
+ config.joint_states_config.joint_names[3] = "r_shoulder_lift_joint";
+ config.joint_states_config.joint_names[4] = "r_upper_arm_roll_joint";
+ config.joint_states_config.joint_names[5] = "r_elbow_flex_joint";
+ config.joint_states_config.joint_names[6] = "r_forearm_roll_joint";
+ config.joint_states_config.joint_names[7] = "r_wrist_flex_joint";
+ config.joint_states_config.joint_names[8] = "r_wrist_roll_joint";
+
+ config.joint_states_config.tolerances.resize(9);
+ config.joint_states_config.tolerances[0] = 0.0;
+ config.joint_states_config.tolerances[1] = 0.0;
+ config.joint_states_config.tolerances[2] = 0.0;
+ config.joint_states_config.tolerances[3] = 0.0;
+ config.joint_states_config.tolerances[4] = 0.0;
+ config.joint_states_config.tolerances[5] = 0.0;
+ config.joint_states_config.tolerances[6] = 0.0;
+ config.joint_states_config.tolerances[7] = 0.0;
+ config.joint_states_config.tolerances[8] = 0.0;
+
+ config.joint_states_config.padding = ros::Duration(1,0);
+ config.joint_states_config.min_samples = 30;
+ config.joint_states_config.cache_size = 450;
+
+ // *************************************
+
+ config.pixel_configs.resize(2);
+ config.pixel_configs[0].pixel_topic = "/stereo/left/led";
+ config.pixel_configs[0].image_topic = "/stereo/left/image_rect";
+ config.pixel_configs[0].channel_name = "Left_Rect";
+ config.pixel_configs[0].tolerance = 1.5;
+ config.pixel_configs[0].padding = ros::Duration().fromSec(.5);
+ config.pixel_configs[0].min_samples = 4;
+
+ config.pixel_configs[1].pixel_topic = "/stereo/right/led";
+ config.pixel_configs[1].image_topic = "/stereo/right/image_rect";
+ config.pixel_configs[1].channel_name = "Right_Rect";
+ config.pixel_configs[1].tolerance = 1.5;
+ config.pixel_configs[1].padding = ros::Duration().fromSec(.5);
+ config.pixel_configs[1].min_samples = 4;
+
+ // *************************************
+
+ config.joint_states_timeshift = ros::Duration(0,0);
+
+ // *************************************
+
+ config.pixel_timeshifts.resize(1);
+ config.pixel_timeshifts[0].chan1 = 0;
+ config.pixel_timeshifts[0].chan2 = 1;
+ config.pixel_timeshifts[0].max_timeshift = ros::Duration(0,0);
+
+ return config;
+}
+
+void activeCallback()
+{
+ ROS_INFO("Request transitioned to ACTIVE!");
+}
+
+void feedbackCallback(const CaptureRobotPixelsFeedbackConstPtr& feedback)
+{
+ printf("%s:\n", feedback->feedback.joint_states_channel.channel_name.c_str());
+ for (unsigned int i=0; i<feedback->feedback.joint_states_channel.ranges.size(); i++)
+ {
+ printf(" ranges[%2u]: %5.3f\n", i, feedback->feedback.joint_states_channel.ranges[i]);
+ }
+
+ for (unsigned int i=0; i<feedback->feedback.pixel_channels.size(); i++)
+ {
+ printf("%s:\n", feedback->feedback.pixel_channels[i].channel_name.c_str());
+ for (unsigned int j=0; j<feedback->feedback.pixel_channels[i].ranges.size(); j++)
+ {
+ printf(" ranges[%2u]: %5.3f\n", i, feedback->feedback.pixel_channels[i].ranges[j]);
+ }
+ }
+}
+
+void doneCallback(const actionlib::TerminalState& terminal_state, const CaptureRobotPixelsResultConstPtr& result)
+{
+ ROS_INFO("DONE! With terminal_state [%s]", terminal_state.toString().c_str());
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "robot_pixel_capture_node");
+
+ ros::NodeHandle nh;
+
+ actionlib::SimpleActionClient<CaptureRobotPixelsAction> ac(nh, "capture_robot_pixels");
+
+ RobotPixelsConfig config = buildCommand();
+ CaptureRobotPixelsGoal full_goal;
+ full_goal.config = config;
+
+ sleep(1);
+
+ ac.sendGoal(full_goal, &doneCallback, &activeCallback, &feedbackCallback);
+
+ ros::spin();
+
+}
+
+
Modified: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/robot_pixels_capture.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/robot_pixels_capture.cpp 2009-08-11 01:23:19 UTC (rev 21495)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/robot_pixels_capture.cpp 2009-08-11 01:30:07 UTC (rev 21496)
@@ -55,6 +55,7 @@
const unsigned int N = config.pixel_configs.size();
pixel_channels_.resize(N);
stationary_pixels_.resize(N);
+ cur_feedback_.pixel_channels.resize(N);
for (unsigned int i=0; i<N; i++)
{
@@ -143,7 +144,7 @@
boost::mutex::scoped_lock lock(joint_states_mutex_);
if (stationary_joint_states_->size() == 0)
{
- ROS_INFO("Haven't received data from JointStates yet");
+ ROS_INFO("Haven't received data any stationary JointStates in a while");
return;
}
success = stationary_joint_states_->getClosestElem(time, joint_states);
@@ -159,7 +160,7 @@
{
if (stationary_pixels_[i]->size() == 0)
{
- ROS_INFO("Haven't received data for PixelChannel[%u] yet", i);
+ ROS_INFO("Haven't received stationary PixelChannel[%u] data in a while", i);
return;
}
success = stationary_pixels_[i]->getClosestElem(time, pixel_vec[i]);
@@ -167,7 +168,6 @@
}
}
-
// If timeshift is 0, then it's a special case, and we skip the timeshift checking
if (joint_states_timeshift_ != ros::Duration(0,0))
{
@@ -189,7 +189,7 @@
}
}
else
- ROS_INFO("Ignote joint states timeshift");
+ ROS_INFO("Ignore joint states timeshift");
// Verify all cross pixel timeshifts are within bounds
for (unsigned int i=0; i<pixel_timeshifts_.size(); i++)
Modified: pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h
===================================================================
--- pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h 2009-08-11 01:23:19 UTC (rev 21495)
+++ pkg/trunk/sandbox/action_translator/include/action_translator/action_translator.h 2009-08-11 01:30:07 UTC (rev 21496)
@@ -43,6 +43,9 @@
namespace action_translator
{
+/**
+ * \brief A helper class used to convert between the old actions (robot_actions) and the new actions (actionlib)
+ */
template<class ActionSpec, class OldGoal, class OldFeedback>
class ActionTranslator : public robot_actions::Action<OldGoal, OldFeedback>
{
@@ -55,6 +58,13 @@
typedef boost::function< OldFeedback(const Feedback&)> FromActionFeedbackFunc;
typedef boost::function< OldFeedback(const Result&)> FromActionResultFunc;
+ /**
+ * \param new_action_name The namespace in which actionlib should communicate. The robot_actions
+ * part will communicate in the namespace [new_action_name]_old
+ * \param from_old_goal_func Function to translate the robot_actions goal into the actionlib goal
+ * \param from_action_feedback_func Function to translate the actionlib feedback into a robot_actions feedback
+ * \param from_action_result_func Function to translate the actionlib result into a robot_actions feedback
+ */
ActionTranslator(const std::string& new_action_name,
FromOldGoalFunc from_old_goal_func,
FromActionFeedbackFunc from_action_feedback_func = FromActionFeedbackFunc(),
@@ -69,6 +79,9 @@
}
+ /**
+ * \brief Blocking call to actionlib with a goal request
+ */
robot_actions::ResultStatus execute(const OldGoal& old_goal, OldFeedback& old_feedback)
{
ac_.stopTrackingGoal();
Modified: pkg/trunk/vision/led_detection/stereo_left_detect.launch
===================================================================
--- pkg/trunk/vision/led_detection/stereo_left_detect.launch 2009-08-11 01:23:19 UTC (rev 21495)
+++ pkg/trunk/vision/led_detection/stereo_left_detect.launch 2009-08-11 01:30:07 UTC (rev 21496)
@@ -5,7 +5,7 @@
<node pkg="led_detection" type="led_detector_node" name="stereo_left_detector" output="screen">
<remap from="image" to="/stereo/left/image_rect" />
<remap from="cam_info" to="/stereo/left/cam_info" />
- <param name="use_led_pose" type="bool" value="true" />
+ <param name="use_led_pose" type="bool" value="false" />
<param name="led_frame" type="string" value="r_gripper_tool_frame" />
<remap from="~led" to="stereo/left/led" />
<remap from="~debug_image" to="stereo/left/led_debug_image" />
Modified: pkg/trunk/vision/led_detection/stereo_right_detect.launch
===================================================================
--- pkg/trunk/vision/led_detection/stereo_right_detect.launch 2009-08-11 01:23:19 UTC (rev 21495)
+++ pkg/trunk/vision/led_detection/stereo_right_detect.launch 2009-08-11 01:30:07 UTC (rev 21496)
@@ -5,7 +5,7 @@
<node pkg="led_detection" type="led_detector_node" name="stereo_right_detector" output="screen">
<remap from="image" to="/stereo/right/image_rect" />
<remap from="cam_info" to="/stereo/right/cam_info" />
- <param name="use_led_pose" type="bool" value="true" />
+ <param name="use_led_pose" type="bool" value="false" />
<param name="led_frame" type="string" value="r_gripper_tool_frame" />
<remap from="~led" to="stereo/right/led" />
<remap from="~debug_image" to="stereo/right/led_debug_image" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
