Menu
▾
▴
[TUM-ros-pkg-commit] SF.net SVN: tum-ros-pkg:[1007] controllers/ias_mechanism_controllers
|
From: <tum...@li...> - 2012-08-07 14:37:07
|
Revision: 1007
http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=1007&view=rev
Author: moesenle
Date: 2012-08-07 14:36:57 +0000 (Tue, 07 Aug 2012)
Log Message:
-----------
Author: Ingo Kresse <kr...@in...>
Date: Tue Aug 7 16:36:08 2012 +0200
publishing multi-joint velocity controller
git-commit
Modified Paths:
--------------
controllers/ias_mechanism_controllers/CMakeLists.txt
controllers/ias_mechanism_controllers/controller_plugins.xml
controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/rosie_odometry.h
controllers/ias_mechanism_controllers/manifest.xml
controllers/ias_mechanism_controllers/src/rosie_odometry.cpp
Added Paths:
-----------
controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h
controllers/ias_mechanism_controllers/launch/
controllers/ias_mechanism_controllers/launch/pr2_loopback.launch
controllers/ias_mechanism_controllers/launch/pr2_multi_vel.yaml
controllers/ias_mechanism_controllers/launch/rosie_loopback.launch
controllers/ias_mechanism_controllers/launch/rosie_multi_vel.yaml
controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp
Modified: controllers/ias_mechanism_controllers/CMakeLists.txt
===================================================================
--- controllers/ias_mechanism_controllers/CMakeLists.txt 2012-07-18 11:05:18 UTC (rev 1006)
+++ controllers/ias_mechanism_controllers/CMakeLists.txt 2012-08-07 14:36:57 UTC (rev 1007)
@@ -10,6 +10,9 @@
#rosbuild_genmsg()
#gensrv()
-rosbuild_add_library(ias_mechanism_controllers src/rosie_odometry.cpp)
+rosbuild_add_library(ias_mechanism_controllers src/rosie_odometry.cpp
+ src/multi_joint_velocity_controller.cpp)
rosbuild_add_boost_directories()
rosbuild_link_boost(ias_mechanism_controllers thread)
+
+
Modified: controllers/ias_mechanism_controllers/controller_plugins.xml
===================================================================
--- controllers/ias_mechanism_controllers/controller_plugins.xml 2012-07-18 11:05:18 UTC (rev 1006)
+++ controllers/ias_mechanism_controllers/controller_plugins.xml 2012-08-07 14:36:57 UTC (rev 1007)
@@ -1,3 +1,4 @@
<library path="lib/libias_mechanism_controllers">
- <class name="RosieOdometry" type="controller::RosieOdometry" base_class_type="pr2_controller_interface::Controller" />
+ <class name="ias_mechanism_controllers/RosieOdometry" type="controller::RosieOdometry" base_class_type="pr2_controller_interface::Controller" />
+ <class name="ias_mechanism_controllers/MultiJointVelocityController" type="controller::MultiJointVelocityController" base_class_type="pr2_controller_interface::Controller" />
</library>
Added: controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h
===================================================================
--- controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h (rev 0)
+++ controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h 2012-08-07 14:36:57 UTC (rev 1007)
@@ -0,0 +1,98 @@
+/*
+ * 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, Inc. 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 Stuart Glaser
+
+ \class pr2_controller_interface::JointTrajectoryActionController
+
+*/
+
+#ifndef MULTI_JOINT_VELOCITY_CONTROLLER_H__
+#define MULTI_JOINT_VELOCITY_CONTROLLER_H__
+
+#include <vector>
+
+#include <boost/scoped_ptr.hpp>
+#include <boost/thread/recursive_mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <ros/node_handle.h>
+
+#include <control_toolbox/pid.h>
+#include <filters/filter_chain.h>
+#include <pr2_controller_interface/controller.h>
+#include <realtime_tools/realtime_publisher.h>
+
+#include <std_msgs/Float64MultiArray.h>
+#include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
+
+
+namespace controller {
+
+
+class MultiJointVelocityController : public pr2_controller_interface::Controller
+{
+public:
+
+ MultiJointVelocityController();
+ ~MultiJointVelocityController();
+
+ bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
+
+ void starting();
+ void update();
+
+private:
+ int loop_count_;
+ pr2_mechanism_model::RobotState *robot_;
+ ros::Time last_time_;
+ std::vector<pr2_mechanism_model::JointState*> joints_;
+ std::vector<control_toolbox::Pid> pids_;
+
+ std::vector<boost::shared_ptr<filters::FilterChain<double> > > output_filters_;
+
+ ros::NodeHandle node_;
+
+ void commandCB(const std_msgs::Float64MultiArray::ConstPtr &msg);
+ ros::Subscriber sub_command_;
+
+ boost::scoped_ptr<
+ realtime_tools::RealtimePublisher<
+ pr2_controllers_msgs::JointTrajectoryControllerState> > controller_state_publisher_;
+
+ // ------ Mechanisms for passing the commands into realtime
+ // (double buffering for commanded velocities, and guarded copy operation)
+
+ boost::mutex command_mutex_;
+ std::vector<double> command_;
+};
+
+}
+
+#endif
Property changes on: controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/multi_joint_velocity_controller.h
___________________________________________________________________
Added: svn:mime-type
+ text/plain
Added: svn:eol-style
+ native
Modified: controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/rosie_odometry.h
===================================================================
--- controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/rosie_odometry.h 2012-07-18 11:05:18 UTC (rev 1006)
+++ controllers/ias_mechanism_controllers/include/ias_mechanism_controllers/rosie_odometry.h 2012-08-07 14:36:57 UTC (rev 1007)
@@ -36,7 +36,8 @@
This file is copied from pr2_mechanism_controllers.
**/
-#include <Eigen/Array>
+#define EIGEN2_SUPPORT
+
#include <Eigen/SVD>
#include <nav_msgs/Odometry.h>
#include <realtime_tools/realtime_publisher.h>
Added: controllers/ias_mechanism_controllers/launch/pr2_loopback.launch
===================================================================
--- controllers/ias_mechanism_controllers/launch/pr2_loopback.launch (rev 0)
+++ controllers/ias_mechanism_controllers/launch/pr2_loopback.launch 2012-08-07 14:36:57 UTC (rev 1007)
@@ -0,0 +1,29 @@
+<launch>
+
+ <param name="/use_sim_time" value="false" />
+
+ <include file="$(find pr2_description)/robots/upload_pr2.launch" />
+
+ <!-- start loopback controller manager -->
+ <node name="loopback_controllers" type="loopback_controller_manager" pkg="loopback_controller_manager" output="screen">
+ <param name="dt" value="0.002" />
+ <param name="damping" value="1.0" />
+ </node>
+
+ <!-- start joint_states to tf transformer -->
+ <node pkg="robot_state_chain_publisher" type="state_chain_publisher" name="robot_state_publisher" output="log" />
+
+
+<!-- <node name="unspawn_arms"
+ pkg="pr2_controller_manager" type="unspawner"
+ args="l_arm_controller r_arm_controller" />
+-->
+
+ <rosparam command="load" file="$(find ias_mechanism_controllers)/launch/pr2_multi_vel.yaml"/>
+
+
+ <node name="spawn_vel"
+ pkg="pr2_controller_manager" type="spawner"
+ args="l_arm_vel r_arm_vel" />
+
+</launch>
Added: controllers/ias_mechanism_controllers/launch/pr2_multi_vel.yaml
===================================================================
--- controllers/ias_mechanism_controllers/launch/pr2_multi_vel.yaml (rev 0)
+++ controllers/ias_mechanism_controllers/launch/pr2_multi_vel.yaml 2012-08-07 14:36:57 UTC (rev 1007)
@@ -0,0 +1,39 @@
+r_arm_vel:
+ type: "ias_mechanism_controllers/MultiJointVelocityController"
+ joints:
+ - r_shoulder_pan_joint
+ - r_shoulder_lift_joint
+ - r_upper_arm_roll_joint
+ - r_elbow_flex_joint
+ - r_forearm_roll_joint
+ - r_wrist_flex_joint
+ - r_wrist_roll_joint
+ gains:
+ r_shoulder_pan_joint: {p: 4, d: 0.0}
+ r_shoulder_lift_joint: {p: 4, d: 0.0}
+ r_upper_arm_roll_joint: {p: 4, d: 0.0}
+ r_elbow_flex_joint: {p: 4, d: 0.0}
+ r_forearm_roll_joint: {p: 4, d: 0.0}
+ r_wrist_flex_joint: {p: 4, d: 0.0}
+ r_wrist_roll_joint: {p: 4, d: 0.0}
+
+
+l_arm_vel:
+ type: "ias_mechanism_controllers/MultiJointVelocityController"
+ joints:
+ - l_shoulder_pan_joint
+ - l_shoulder_lift_joint
+ - l_upper_arm_roll_joint
+ - l_elbow_flex_joint
+ - l_forearm_roll_joint
+ - l_wrist_flex_joint
+ - l_wrist_roll_joint
+ gains:
+ l_shoulder_pan_joint: {p: 4, d: 0.0}
+ l_shoulder_lift_joint: {p: 4, d: 0.0}
+ l_upper_arm_roll_joint: {p: 4, d: 0.0}
+ l_elbow_flex_joint: {p: 4, d: 0.0}
+ l_forearm_roll_joint: {p: 4, d: 0.0}
+ l_wrist_flex_joint: {p: 4, d: 0.0}
+ l_wrist_roll_joint: {p: 4, d: 0.0}
+
Added: controllers/ias_mechanism_controllers/launch/rosie_loopback.launch
===================================================================
--- controllers/ias_mechanism_controllers/launch/rosie_loopback.launch (rev 0)
+++ controllers/ias_mechanism_controllers/launch/rosie_loopback.launch 2012-08-07 14:36:57 UTC (rev 1007)
@@ -0,0 +1,29 @@
+<launch>
+
+ <param name="/use_sim_time" value="false" />
+
+ <include file="$(find rosie_description)/launch/upload_rosie.launch" />
+
+ <!-- start loopback controller manager -->
+ <node name="loopback_controllers" type="loopback_controller_manager" pkg="loopback_controller_manager" output="screen">
+ <param name="dt" value="0.002" />
+ <param name="damping" value="1.0" />
+ </node>
+
+ <!-- start joint_states to tf transformer -->
+ <node pkg="robot_state_chain_publisher" type="state_chain_publisher" name="robot_state_publisher" output="log" />
+
+
+<!-- <node name="unspawn_arms"
+ pkg="pr2_controller_manager" type="unspawner"
+ args="l_arm_controller r_arm_controller" />
+-->
+
+ <rosparam command="load" file="$(find ias_mechanism_controllers)/launch/rosie_multi_vel.yaml"/>
+
+
+ <node name="spawn_vel"
+ pkg="pr2_controller_manager" type="spawner"
+ args="l_arm_vel r_arm_vel" />
+
+</launch>
Added: controllers/ias_mechanism_controllers/launch/rosie_multi_vel.yaml
===================================================================
--- controllers/ias_mechanism_controllers/launch/rosie_multi_vel.yaml (rev 0)
+++ controllers/ias_mechanism_controllers/launch/rosie_multi_vel.yaml 2012-08-07 14:36:57 UTC (rev 1007)
@@ -0,0 +1,39 @@
+r_arm_vel:
+ type: "ias_mechanism_controllers/MultiJointVelocityController"
+ joints:
+ - right_arm_0_joint
+ - right_arm_1_joint
+ - right_arm_2_joint
+ - right_arm_3_joint
+ - right_arm_4_joint
+ - right_arm_5_joint
+ - right_arm_6_joint
+ gains:
+ right_arm_0_joint: {p: 4, d: 0.0}
+ right_arm_1_joint: {p: 4, d: 0.0}
+ right_arm_2_joint: {p: 4, d: 0.0}
+ right_arm_3_joint: {p: 4, d: 0.0}
+ right_arm_4_joint: {p: 4, d: 0.0}
+ right_arm_5_joint: {p: 4, d: 0.0}
+ right_arm_6_joint: {p: 4, d: 0.0}
+
+
+l_arm_vel:
+ type: "ias_mechanism_controllers/MultiJointVelocityController"
+ joints:
+ - left_arm_0_joint
+ - left_arm_1_joint
+ - left_arm_2_joint
+ - left_arm_3_joint
+ - left_arm_4_joint
+ - left_arm_5_joint
+ - left_arm_6_joint
+ gains:
+ left_arm_0_joint: {p: 4.0}
+ left_arm_1_joint: {p: 4.0}
+ left_arm_2_joint: {p: 4.0}
+ left_arm_3_joint: {p: 4.0}
+ left_arm_4_joint: {p: 4.0}
+ left_arm_5_joint: {p: 4.0}
+ left_arm_6_joint: {p: 4.0}
+
Modified: controllers/ias_mechanism_controllers/manifest.xml
===================================================================
--- controllers/ias_mechanism_controllers/manifest.xml 2012-07-18 11:05:18 UTC (rev 1006)
+++ controllers/ias_mechanism_controllers/manifest.xml 2012-08-07 14:36:57 UTC (rev 1007)
@@ -1,11 +1,15 @@
<package>
<description brief="ias_mechanism_controllers">
Mechanism controllers for ias related stuff
+ (Odometry publisher for simulated ROSIE and a
+ multi-joint velocity controller.)
</description>
- <author>Lorenz Moesenlechner</author>
+ <author>Lorenz Moesenlechner, Ingo Kresse</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
+ <depend package="pluginlib" />
+
<depend package="roscpp" />
<depend package="std_msgs" />
<depend package="pr2_controller_interface" />
@@ -13,7 +17,9 @@
<depend package="pr2_mechanism_controllers" />
<depend package="pr2_mechanism_msgs" />
<depend package="nav_msgs" />
+ <depend package="filters" />
<depend package="control_toolbox" />
+ <depend package="realtime_tools" />
<depend package="pr2_controller_manager" />
<export>
Added: controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp
===================================================================
--- controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp (rev 0)
+++ controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp 2012-08-07 14:36:57 UTC (rev 1007)
@@ -0,0 +1,210 @@
+/*
+ * 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, Inc. 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: Stuart Glaser
+ */
+
+#include "ias_mechanism_controllers/multi_joint_velocity_controller.h"
+#include <sstream>
+#include "angles/angles.h"
+#include "pluginlib/class_list_macros.h"
+
+PLUGINLIB_DECLARE_CLASS(ias_mechanism_controllers, MultiJointVelocityController, controller::MultiJointVelocityController, pr2_controller_interface::Controller)
+
+namespace controller {
+
+
+MultiJointVelocityController::MultiJointVelocityController()
+ : loop_count_(0), robot_(NULL)
+{
+}
+
+MultiJointVelocityController::~MultiJointVelocityController()
+{
+ sub_command_.shutdown();
+}
+
+bool MultiJointVelocityController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
+{
+ using namespace XmlRpc;
+ node_ = n;
+ robot_ = robot;
+
+ // Gets all of the joints
+ XmlRpc::XmlRpcValue joint_names;
+ if (!node_.getParam("joints", joint_names))
+ {
+ ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
+ return false;
+ }
+ if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
+ {
+ ROS_ERROR("Malformed joint specification. (namespace: %s)", node_.getNamespace().c_str());
+ return false;
+ }
+ for (int i = 0; i < joint_names.size(); ++i)
+ {
+ XmlRpcValue &name_value = joint_names[i];
+ if (name_value.getType() != XmlRpcValue::TypeString)
+ {
+ ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)",
+ node_.getNamespace().c_str());
+ return false;
+ }
+
+ pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
+ if (!j) {
+ ROS_ERROR("Joint not found: %s. (namespace: %s)",
+ ((std::string)name_value).c_str(), node_.getNamespace().c_str());
+ return false;
+ }
+ joints_.push_back(j);
+ }
+
+ // Ensures that all the joints are calibrated.
+ for (size_t i = 0; i < joints_.size(); ++i)
+ {
+ if (!joints_[i]->calibrated_)
+ {
+ ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
+ joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
+ return false;
+ }
+ }
+
+ // Sets up pid controllers for all of the joints
+ std::string gains_ns;
+ if (!node_.getParam("gains", gains_ns))
+ gains_ns = node_.getNamespace() + "/gains";
+ pids_.resize(joints_.size());
+ for (size_t i = 0; i < joints_.size(); ++i)
+ if (!pids_[i].init(ros::NodeHandle(gains_ns + "/" + joints_[i]->joint_->name)))
+ return false;
+
+
+ output_filters_.resize(joints_.size());
+ for (size_t i = 0; i < joints_.size(); ++i)
+ {
+ std::string p = "output_filters/" + joints_[i]->joint_->name;
+ if (node_.hasParam(p))
+ {
+ output_filters_[i].reset(new filters::FilterChain<double>("double"));
+ if (!output_filters_[i]->configure(node_.resolveName(p)))
+ output_filters_[i].reset();
+ }
+ }
+
+ sub_command_ = node_.subscribe("command", 1, &MultiJointVelocityController::commandCB, this);
+
+ command_.resize(joints_.size());
+
+ controller_state_publisher_.reset(
+ new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointTrajectoryControllerState>
+ (node_, "state", 1));
+ controller_state_publisher_->lock();
+ for (size_t j = 0; j < joints_.size(); ++j)
+ controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
+ controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
+ controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
+ controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
+ controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
+ controller_state_publisher_->unlock();
+
+
+ return true;
+}
+
+void MultiJointVelocityController::starting()
+{
+ last_time_ = robot_->getTime();
+
+ for (size_t i = 0; i < pids_.size(); ++i)
+ pids_[i].reset();
+
+ // Creates a "hold current position" trajectory.
+}
+
+void MultiJointVelocityController::update()
+{
+ ros::Time time = robot_->getTime();
+ ros::Duration dt = time - last_time_;
+ last_time_ = time;
+
+ double command[joints_.size()];
+
+ boost::mutex::scoped_lock guard(command_mutex_);
+ for(unsigned int i=0; i < command_.size(); ++i)
+ command[i] = command_[i];
+ guard.unlock();
+
+ double error[joints_.size()];
+ for (size_t i = 0; i < joints_.size(); ++i)
+ {
+ error[i] = joints_[i]->velocity_ - command[i];
+ double effort = pids_[i].updatePid(error[i], dt);
+ double effort_filtered = effort;
+ if (output_filters_[i])
+ output_filters_[i]->update(effort, effort_filtered);
+ joints_[i]->commanded_effort_ += effort_filtered;
+ }
+
+
+ // ------ State publishing
+
+ if (loop_count_ % 10 == 0)
+ {
+ if (controller_state_publisher_ && controller_state_publisher_->trylock())
+ {
+ controller_state_publisher_->msg_.header.stamp = time;
+ for (size_t j = 0; j < joints_.size(); ++j)
+ {
+ controller_state_publisher_->msg_.desired.velocities[j] = command[j];
+ controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
+ controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
+ controller_state_publisher_->msg_.error.velocities[j] = error[j];
+ }
+ controller_state_publisher_->unlockAndPublish();
+ }
+ }
+
+ ++loop_count_;
+}
+
+void MultiJointVelocityController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &msg)
+{
+ boost::mutex::scoped_lock guard(command_mutex_);
+ for(unsigned int i=0; i < command_.size(); ++i)
+ command_[i] = msg->data[i];
+}
+
+
+
+
+}
Property changes on: controllers/ias_mechanism_controllers/src/multi_joint_velocity_controller.cpp
___________________________________________________________________
Added: svn:mime-type
+ text/plain
Added: svn:eol-style
+ native
Modified: controllers/ias_mechanism_controllers/src/rosie_odometry.cpp
===================================================================
--- controllers/ias_mechanism_controllers/src/rosie_odometry.cpp 2012-07-18 11:05:18 UTC (rev 1006)
+++ controllers/ias_mechanism_controllers/src/rosie_odometry.cpp 2012-08-07 14:36:57 UTC (rev 1007)
@@ -38,7 +38,7 @@
#include "ias_mechanism_controllers/rosie_odometry.h"
#include "pluginlib/class_list_macros.h"
-PLUGINLIB_REGISTER_CLASS(RosieOdometry, controller::RosieOdometry, pr2_controller_interface::Controller)
+PLUGINLIB_DECLARE_CLASS(ias_mechanism_controllers, RosieOdometry, controller::RosieOdometry, pr2_controller_interface::Controller)
namespace controller {
@@ -93,7 +93,7 @@
odometry_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(node_,odom_frame_, 1));
transform_publisher_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(node_,"/tf", 1));
- transform_publisher_->msg_.set_transforms_size(1);
+ transform_publisher_->msg_.transforms.resize(1);
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|