Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14753] pkg/trunk
|
From: <stu...@us...> - 2009-05-01 18:52:14
|
Revision: 14753
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14753&view=rev
Author: stuglaser
Date: 2009-05-01 18:52:11 +0000 (Fri, 01 May 2009)
Log Message:
-----------
Moved the plug detector outside of the actions.
Modified Paths:
--------------
pkg/trunk/demos/plug_in/integration/controllers.launch
pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h
pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp
pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp
pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp
Added Paths:
-----------
pkg/trunk/highlevel/plugs/plugs_core/msg/
pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg
Modified: pkg/trunk/demos/plug_in/integration/controllers.launch
===================================================================
--- pkg/trunk/demos/plug_in/integration/controllers.launch 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/demos/plug_in/integration/controllers.launch 2009-05-01 18:52:11 UTC (rev 14753)
@@ -1,5 +1,5 @@
<launch>
-
+
<group ns="prosilica">
<include file="$(find prosilica_cam)/cam_settings.xml"/>
<param name="acquisition_mode" type="str" value="Triggered"/>
@@ -11,26 +11,26 @@
<group ns="r_arm_joint_trajectory_controller" clear_params="true">
<param name="velocity_scaling_factor" value="0.25" type="double" />
<param name="trajectory_wait_timeout" value="10.0" type="double" />
-
+
<param name="r_shoulder_pan_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_shoulder_lift_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_upper_arm_roll_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_elbow_flex_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_forearm_roll_joint/goal_reached_threshold" value="0.05" type="double" />
<param name="r_wrist_flex_joint/goal_reached_threshold" value="0.05" type="double" />
- <param name="r_wrist_roll_joint/goal_reached_threshold" value="0.05" type="double" />
- <param name="autostart" value="false" type="bool" />
+ <param name="r_wrist_roll_joint/goal_reached_threshold" value="0.05" type="double" />
+ <param name="autostart" value="false" type="bool" />
</group>
-
-
+
+
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/r_arm_joint_trajectory_controller.xml" />
-
+
<!-- Controller and Node Required for plug on base detection -->
-
-
- <param name="laser_tilt_controller/autostart" value="false" type="bool" />
+
+
+ <param name="laser_tilt_controller/autostart" value="false" type="bool" />
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_experimental_controllers)/laser_tilt/laser_tilt_controller.xml" />
-
+
<node pkg="point_cloud_assembler" type="laser_scan_assembler_srv" output="screen" name="laser_scan_assembler" respawn="true">
<remap from="scan_in" to="tilt_scan"/>
<param name="tf_cache_time_secs" type="double" value="10.0" />
@@ -44,14 +44,14 @@
<remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
<remap from="build_cloud" to="laser_scan_assembler/build_cloud" />
<remap from="full_cloud" to="snapshot_cloud" />
- </node>
-
+ </node>
+
<!-- Controller and Node Required for move and grasp -->
<!-- Cartesian wrench controller -->
<group ns="r_arm_cartesian_wrench_controller" clear_params="true">
<param name="root_name" type="string" value="torso_lift_link" />
<param name="tip_name" type="string" value="r_gripper_tool_frame" />
- <param name="autostart" value="false" type="bool" />
+ <param name="autostart" value="false" type="bool" />
</group>
<!-- Cartesian twist controller -->
@@ -72,7 +72,7 @@
<param name="fb_rot/i" value="0.1" />
<param name="fb_rot/d" value="0.0" />
<param name="fb_rot/i_clamp" value="0.2" />
- <param name="autostart" value="false" type="bool" />
+ <param name="autostart" value="false" type="bool" />
</group>
<!-- Cartesian pose controller -->
@@ -85,7 +85,7 @@
<param name="i" value="0.1" />
<param name="d" value="0.0" />
<param name="i_clamp" value="0.5" />
- <param name="autostart" value="false" type="bool" />
+ <param name="autostart" value="false" type="bool" />
</group>
<!-- Cartesian trajectory controller -->
@@ -98,17 +98,17 @@
<param name="max_vel_rot" value="1.0" />
<param name="max_acc_trans" value="0.2" />
<param name="max_acc_rot" value="0.4" />
- <param name="autostart" value="false" type="bool" />
+ <param name="autostart" value="false" type="bool" />
</group>
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/r_arm_cartesian_wrench_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_twist_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_pose_controller.xml $(find pr2_default_controllers)/r_arm_cartesian_trajectory_controller.xml" />
<!--Gripper position controller-->
- <param name="r_gripper_position_controller/autostart" value="false" type="bool" />
+ <param name="r_gripper_position_controller/autostart" value="false" type="bool" />
<node pkg="mechanism_control" type="spawner.py" args="$(find plug_in)/gripper_controller.xml"/>
-
-
+
+
<!-- Parameters for the controllers used in plugging in -->
<group ns="r_arm_hybrid_controller" clear_params="true">
@@ -123,10 +123,14 @@
<rosparam file="$(find pr2_default_controllers)/gains/arm_twist.yaml" command="load" />
</group>
<param name="twist_filter" command="./../make_lowpass.m twist_filter 0.01" />
- <param name="saturated_velocity" value="0.5" />
+ <param name="saturated_velocity" value="0.4" />
+ <!--<param name="saturated_rot_velocity" value="0.8" />-->
<param name="autostart" value="false" type="bool" />
</group>
<node pkg="mechanism_control" type="spawner.py" args="$(find plug_in)/integration/r_arm_hybrid_controller.xml"/>
+ <!-- Plug tracking node -->
+ <include file="$(find outlet_detection)/launch/plug.launch" />
+
</launch>
Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h 2009-05-01 18:52:11 UTC (rev 14753)
@@ -46,6 +46,7 @@
#include "robot_msgs/PoseStamped.h"
#include "robot_msgs/CartesianState.h"
#include "robot_msgs/TaskFrameFormalism.h"
+#include "plugs_core/PlugInState.h"
//TF
#include <tf/transform_listener.h>
@@ -70,7 +71,7 @@
robot_actions::ResultStatus execute(const std_msgs::Empty& empty, std_msgs::Empty& feedback);
private:
-
+
void reset();
void plugMeasurementCallback(const tf::MessageNotifier<robot_msgs::PoseStamped>::MessagePtr &msg);
void measure();
@@ -79,28 +80,27 @@
void force();
void insert();
-
+
std::string action_name_;
ros::Node& node_;
std::string arm_controller_;
-
+
PlugTracker::PlugTracker* detector_;
std_msgs::Empty empty_;
-
+
boost::scoped_ptr<tf::MessageNotifier<robot_msgs::PoseStamped> > notifier_;
boost::scoped_ptr<tf::TransformListener> TF_;
-
- double last_standoff_;
+
+ double last_standoff_;
ros::Time g_started_inserting_, g_started_forcing_, g_stopped_forcing_;
int g_state_;
int prev_state_;
-
+
tf::Stamped<tf::Transform> mech_offset_;
tf::Pose mech_offset_desi_;
- robot_msgs::TaskFrameFormalism tff_msg_;
-
+ robot_msgs::TaskFrameFormalism tff_msg_;
};
}
Added: pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg (rev 0)
+++ pkg/trunk/highlevel/plugs/plugs_core/msg/PlugInState.msg 2009-05-01 18:52:11 UTC (rev 14753)
@@ -0,0 +1,6 @@
+Header header
+int32 state
+int32 next_state
+
+robot_msgs/Twist viz_offset
+robot_msgs/Twist viz_error
\ No newline at end of file
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_localize_plug_in_gripper.cpp 2009-05-01 18:52:11 UTC (rev 14753)
@@ -43,6 +43,7 @@
robot_actions::Action<std_msgs::Empty, std_msgs::Empty>("localize_plug_in_gripper"),
action_name_("localize_plug_in_gripper"),
node_(node),
+ detector_(NULL),
arm_controller_("r_arm_cartesian_trajectory_controller"),
servoing_controller_("r_arm_hybrid_controller"),
TF(*ros::Node::instance(),false, ros::Duration(10))
@@ -59,24 +60,24 @@
node_.param(action_name_ + "/arm_controller", arm_controller_, arm_controller_);
if(arm_controller_ == "" )
- {
- ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
- terminate();
- return;
- }
+ {
+ ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
+ terminate();
+ return;
+ }
node_.param(action_name_ + "/servoing_controller", servoing_controller_, servoing_controller_);
if(servoing_controller_ == "" )
- {
- ROS_ERROR("%s: Aborted, servoing controller param was not set.", action_name_.c_str());
- terminate();
- return;
- }
+ {
+ ROS_ERROR("%s: Aborted, servoing controller param was not set.", action_name_.c_str());
+ terminate();
+ return;
+ }
- detector_ = new PlugTracker::PlugTracker(node);
- detector_->deactivate();
- node_.subscribe("~plug_pose", plug_pose_msg_, &LocalizePlugInGripperAction::setToolFrame, this, 1);
+ //detector_ = new PlugTracker::PlugTracker(node);
+ //detector_->deactivate();
+ node_.subscribe("/plug_detector/plug_pose", plug_pose_msg_, &LocalizePlugInGripperAction::setToolFrame, this, 1);
};
LocalizePlugInGripperAction::~LocalizePlugInGripperAction()
@@ -156,7 +157,8 @@
return;
}
- detector_->activate();
+ if (detector_)
+ detector_->activate();
return;
}
@@ -169,7 +171,8 @@
if (isPreemptRequested())
{
- detector_->deactivate();
+ if (detector_)
+ detector_->deactivate();
deactivate(robot_actions::PREEMPTED, std_msgs::Empty());
return;
}
@@ -183,8 +186,8 @@
return;
}
-
- detector_->deactivate();
+ if (detector_)
+ detector_->deactivate();
ROS_INFO("%s: succeeded.", action_name_.c_str());
deactivate(robot_actions::SUCCESS, empty_);
}
Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp 2009-05-01 18:52:11 UTC (rev 14753)
@@ -27,7 +27,7 @@
* 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 INeco
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING INeco
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
@@ -46,7 +46,8 @@
robot_actions::Action<std_msgs::Empty, std_msgs::Empty>("plug_in"),
action_name_("plug_in"),
node_(node),
- arm_controller_("r_arm_hybrid_controller")
+ arm_controller_("r_arm_hybrid_controller"),
+ detector_(NULL)
{
node_.setParam("~roi_policy", "LastImageLocation");
node_.setParam("~display", 0);
@@ -57,28 +58,26 @@
node_.param(action_name_ + "/arm_controller", arm_controller_, arm_controller_);
if(arm_controller_ == "" )
- {
- ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
- terminate();
- return;
- }
+ {
+ ROS_ERROR("%s: Aborted, arm controller param was not set.", action_name_.c_str());
+ terminate();
+ return;
+ }
- node_.advertise<robot_msgs::TaskFrameFormalism>(arm_controller_ + "/command", 2);
+ node_.advertise<robot_msgs::TaskFrameFormalism>(arm_controller_ + "/command", 2);
- detector_ = new PlugTracker::PlugTracker(node);
- detector_->deactivate();
+ //detector_ = new PlugTracker::PlugTracker(node);
+ //detector_->deactivate();
- TF_.reset(new tf::TransformListener(node));
-// notifier_.reset(new tf::MessageNotifier<robot_msgs::PoseStamped>(
-// TF_.get(), &node, PlugInAction::plugMeasurementCallback, "~pose", "outlet_pose", 100));
- notifier_.reset(new tf::MessageNotifier<robot_msgs::PoseStamped>(
- TF_.get(), &node,\
-boost::bind(&PlugInAction::plugMeasurementCallback, this, _1),
-//PlugInAction::plugMeasurementCallback,
-"~plug_pose", "outlet_pose", 100));
+ TF_.reset(new tf::TransformListener(node));
+ notifier_.reset(new tf::MessageNotifier<robot_msgs::PoseStamped>(
+ TF_.get(), &node,
+ boost::bind(&PlugInAction::plugMeasurementCallback, this, _1),
+ "/plug_detector/plug_pose", "outlet_pose", 100));
tff_msg_.header.frame_id = "outlet_pose";
+ node_.advertise<plugs_core::PlugInState>(action_name_ + "/state", 10);
};
PlugInAction::~PlugInAction()
@@ -89,7 +88,8 @@
robot_actions::ResultStatus PlugInAction::execute(const std_msgs::Empty& empty, std_msgs::Empty& feedback)
{
reset();
- detector_->activate();
+ if (detector_)
+ detector_->activate();
return waitForDeactivation(feedback);
}
@@ -103,18 +103,27 @@
g_stopped_forcing_ = ros::Time::now();
}
+void PoseTFToMsg(const tf::Pose &p, robot_msgs::Twist &t)
+{
+ t.vel.x = p.getOrigin().x();
+ t.vel.y = p.getOrigin().y();
+ t.vel.z = p.getOrigin().z();
+ btMatrix3x3(p.getRotation()).getEulerYPR(t.rot.x, t.rot.y, t.rot.z);
+}
void PlugInAction::plugMeasurementCallback(const tf::MessageNotifier<robot_msgs::PoseStamped>::MessagePtr &msg)
{
+ plugs_core::PlugInState state_msg;
ROS_INFO("recieved plug_pose Msg in callback");
if (!isActive())
return;
-
+
if (isPreemptRequested()){
deactivate(robot_actions::PREEMPTED, empty_);
- detector_->deactivate();
+ if (detector_)
+ detector_->deactivate();
return;
}
tff_msg_.header.stamp = msg->header.stamp;
@@ -133,22 +142,25 @@
tf::Pose viz_offset;
tf::PoseMsgToTF(viz_offset_msg.pose, viz_offset);
- double standoff = std::max(MIN_STANDOFF, viz_offset.getOrigin().length() * 2.5/4.0);
-
+ PoseTFToMsg(viz_offset, state_msg.viz_offset);
+ double standoff = std::max(MIN_STANDOFF, viz_offset.getOrigin().length() * 2.5/4.0);
+
// Computes the offset for movement
tf::Pose viz_offset_desi;
viz_offset_desi.setIdentity();
viz_offset_desi.getOrigin().setX(-standoff);
+ viz_offset_desi.getOrigin().setY(-0.003);
viz_offset_desi.getOrigin().setZ(-0.004);
+ PoseTFToMsg(viz_offset.inverse() * viz_offset_desi, state_msg.viz_error);
mech_offset_desi_ = viz_offset.inverse() * viz_offset_desi * mech_offset_;
prev_state_ = g_state_;
switch (g_state_) {
- case MEASURING:
+ case MEASURING:
{
if (viz_offset.getOrigin().length() > 0.5 ||
- viz_offset.getRotation().getAngle() > (M_PI/4.0))
+ viz_offset.getRotation().getAngle() > (M_PI/6.0))
{
double ypr[3];
btMatrix3x3(viz_offset.getRotation()).getEulerYPR(ypr[2], ypr[1], ypr[0]);
@@ -160,7 +172,7 @@
double error = sqrt(pow(viz_offset.getOrigin().y() - viz_offset_desi.getOrigin().y(), 2) +
pow(viz_offset.getOrigin().z() - viz_offset_desi.getOrigin().z(), 2));
- ROS_DEBUG("%s: Error = %0.6lf.", action_name_.c_str(), error);
+ ROS_DEBUG("%s: Error = %0.6lf.", action_name_.c_str(), error);
if (error < 0.002 && last_standoff_ < MIN_STANDOFF + 0.002)
g_state_ = INSERTING;
else
@@ -174,7 +186,7 @@
break;
}
- case INSERTING:
+ case INSERTING:
{
tf::Vector3 offset = viz_offset.getOrigin() - viz_offset_desi.getOrigin();
ROS_DEBUG("%s: Offset: (% 0.3lf, % 0.3lf, % 0.3lf)", action_name_.c_str(), offset.x(), offset.y(), offset.z());
@@ -191,22 +203,25 @@
}
break;
}
- case FORCING:
+ case FORCING:
{
if (ros::Time::now() > g_started_forcing_ + ros::Duration(1.0))
g_state_ = HOLDING;
break;
}
- case HOLDING:
+ case HOLDING:
{
break;
}
}
+ state_msg.state = prev_state_;
+ state_msg.next_state = g_state_;
+
if (g_state_ != prev_state_)
{
switch (g_state_) {
- case MEASURING:
+ case MEASURING:
{
ROS_DEBUG("MEASURING");
if (prev_state_ == INSERTING || prev_state_ == FORCING)
@@ -215,36 +230,37 @@
}
break;
}
- case MOVING:
+ case MOVING:
{
ROS_DEBUG("MOVING");
move();
break;
}
- case INSERTING:
+ case INSERTING:
{
ROS_DEBUG("INSERTING");
insert();
break;
}
- case FORCING:
+ case FORCING:
{
ROS_DEBUG("FORCING");
force();
break;
}
- case HOLDING:
+ case HOLDING:
{
ROS_DEBUG("HOLDING");
hold();
deactivate(robot_actions::SUCCESS, empty_);
- detector_->deactivate();
+ if (detector_)
+ detector_->deactivate();
break;
}
}
}
-
+
last_standoff_ = standoff;
return;
}
@@ -316,7 +332,7 @@
{
if (!isActive())
return;
-
+
g_started_forcing_ = ros::Time::now();
tff_msg_.mode.vel.x = 1;
@@ -335,6 +351,19 @@
tff_msg_.mode.vel.z = 2;
tff_msg_.value.vel.y = 0.0;
tff_msg_.value.vel.z = 0.0;
+
+
+ tff_msg_.mode.vel.x = 1;
+ tff_msg_.mode.vel.y = 3;
+ tff_msg_.mode.vel.z = 3;
+ tff_msg_.mode.rot.x = 3;
+ tff_msg_.mode.rot.y = 3;
+ tff_msg_.mode.rot.z = 3;
+ tff_msg_.value.vel.x = 30;
+ tff_msg_.value.vel.y = mech_offset_.getOrigin().y();
+ tff_msg_.value.vel.z = mech_offset_.getOrigin().z();
+ mech_offset_desi_.getBasis().getEulerZYX(tff_msg_.value.rot.z, tff_msg_.value.rot.y, tff_msg_.value.rot.x);
+
node_.publish(arm_controller_ + "/command", tff_msg_);
return;
}
@@ -343,7 +372,7 @@
{
if (!isActive())
return;
-
+
tff_msg_.mode.vel.x = 1;
tff_msg_.mode.vel.y = 3;
tff_msg_.mode.vel.z = 3;
Modified: pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp 2009-05-01 18:51:49 UTC (rev 14752)
+++ pkg/trunk/highlevel/plugs/plugs_core/test/test_executive_hybrid.cpp 2009-05-01 18:52:11 UTC (rev 14753)
@@ -1,13 +1,13 @@
/*********************************************************************
* 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
@@ -17,7 +17,7 @@
* * Neither the name of 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
@@ -83,14 +83,14 @@
ros::Node node("test_executive");
boost::thread_group threads_;
-
+
pr2_robot_actions::SwitchControllers switchlist;
std_msgs::Empty empty;
- robot_msgs::PlugStow plug_stow;
+ robot_msgs::PlugStow plug_stow;
robot_msgs::PointStamped point;
robot_msgs::PoseStamped pose;
-
+
point.header.frame_id = "torso_lift_link";
point.point.x=0;
point.point.y=0;
@@ -112,7 +112,7 @@
robot_actions::ActionClient< robot_msgs::PlugStow, pr2_robot_actions::StowPlugState, std_msgs::Empty> stow_plug("stow_plug");
- timeout_medium.sleep();
+ timeout_short.sleep();
#if 0
// tuck arm
// switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
@@ -143,7 +143,7 @@
// detect outlet fine
if (detect_outlet_fine.execute(point, pose, timeout_long) != robot_actions::SUCCESS) return -1;
-
+
// localize plug in gripper
if (localize_plug_in_gripper.execute(empty, empty, timeout_long) != robot_actions::SUCCESS) return -1;
@@ -157,11 +157,12 @@
if (switch_controllers.execute(switchlist, empty, timeout_short) != robot_actions::SUCCESS) return -1;
if (plug_in.execute(empty, empty, timeout_long) != robot_actions::SUCCESS) return -1;
- timeout_long.sleep();
-#if 0
+ Duration().fromSec(5.0).sleep();
+
//unplug
if (unplug.execute(empty, empty, timeout_long) != robot_actions::SUCCESS) return -1;
+#if 0
//stow plug
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_hybrid_controller");
@@ -175,6 +176,7 @@
#endif
+ timeout_long.sleep();
// stop remaining controllers
switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
switchlist.stop_controllers.push_back("r_arm_hybrid_controller");
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
