Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Removed outdated controllers, added controllers to be merged upstream #26

Merged
merged 2 commits into from
Aug 25, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 8 additions & 3 deletions pr_ros_controllers/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,9 @@ find_package(catkin REQUIRED COMPONENTS
controller_interface
rosconsole
roscpp
pr_control_msgs
actionlib
actionlib_msgs
)

find_package(Boost REQUIRED COMPONENTS system)
Expand All @@ -16,14 +19,16 @@ link_directories(${Boost_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS})
catkin_package(
CATKIN_DEPENDS
controller_interface
actionlib
actionlib_msgs
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)

add_library(${PROJECT_NAME}
src/pr_joint_position_controller.cpp
src/pr_joint_velocity_controller.cpp
src/joint_mode_controller.cpp
src/joint_group_position_controller.cpp
src/joint_group_velocity_controller.cpp
src/joint_group_effort_controller.cpp
)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,320 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2021, Personal Robotics Lab
//
// 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 Personal Robotics Lab 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 Ethan Kroll Gordon

#pragma once


namespace forward_command_controller
{

namespace internal
{
// TODO: create a utils file?
/**
* \return The map between \p t1 indices (implicitly encoded in return vector indices) to \t2 indices.
* If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated mapping vector is
* <tt>"{2, 1}"</tt>.
*/
template <class T>
inline std::vector<unsigned int> mapping(const T& t1, const T& t2)
{
typedef unsigned int SizeType;

// t1 must be a subset of t2
if (t1.size() > t2.size()) {return std::vector<SizeType>();}

std::vector<SizeType> mapping_vector(t1.size()); // Return value
for (typename T::const_iterator t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
{
typename T::const_iterator t2_it = std::find(t2.begin(), t2.end(), *t1_it);
if (t2.end() == t2_it) {return std::vector<SizeType>();}
else
{
const SizeType t1_dist = std::distance(t1.begin(), t1_it);
const SizeType t2_dist = std::distance(t2.begin(), t2_it);
mapping_vector[t1_dist] = t2_dist;
}
}
return mapping_vector;
}

inline std::string getLeafNamespace(const ros::NodeHandle& nh)
{
const std::string complete_ns = nh.getNamespace();
std::size_t id = complete_ns.find_last_of("/");
return complete_ns.substr(id + 1);
}

} // namespace

template <class HardwareInterface>
bool ForwardJointGroupCommandController<HardwareInterface>::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& n)
{
// Get base hardware interface
HardwareInterface* hw = robot_hw->get<HardwareInterface>();

// Cache controller node handle
controller_nh_ = n;

// Controller name
name_ = internal::getLeafNamespace(controller_nh_);

// Action status checking update rate
double action_monitor_rate = 20.0;
controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
action_monitor_period_ = ros::Duration(1.0 / action_monitor_rate);
ROS_DEBUG_STREAM_NAMED(name_, "Action status changes will be monitored at " << action_monitor_rate << "Hz.");

// Initialize controlled joints
std::string param_name = "joints";
if(!n.getParam(param_name, joint_names_))
{
ROS_ERROR_STREAM("Failed to getParam '" << param_name << "' (namespace: " << n.getNamespace() << ").");
return false;
}
n_joints_ = joint_names_.size();

if(n_joints_ == 0){
ROS_ERROR_STREAM("List of joint names is empty.");
return false;
}

// Clear joints_ first in case this is called twice
joints_.clear();
for(unsigned int i=0; i<n_joints_; i++)
{
try
{
joints_.push_back(hw->getHandle(joint_names_[i]));
}
catch (const hardware_interface::HardwareInterfaceException& e)
{
ROS_ERROR_STREAM("Exception thrown: " << e.what());
return false;
}
}

commands_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));
default_commands_.resize(n_joints_);

// ROS API: Subscribed topics
sub_command_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &ForwardJointGroupCommandController::commandCB, this);

// ROS API: Action interface
action_server_.reset(new ActionServer(controller_nh_, "joint_group_command",
boost::bind(&ForwardJointGroupCommandController::goalCB, this, _1),
boost::bind(&ForwardJointGroupCommandController::cancelCB, this, _1),
false));
action_server_->start();

// Add Joint Mode switching if handle provided
hardware_interface::JointModeInterface* jmt = robot_hw->get<hardware_interface::JointModeInterface>();
std::string handle_name;
if(jmt && n.getParam("mode_handle", handle_name)) {
ROS_DEBUG_STREAM_NAMED(name_, "Enabling joint mode handling.");
mode_handle_.reset(new hardware_interface::JointModeHandle(jmt->getHandle(handle_name)));
}
return true;
}

template <class HardwareInterface>
void ForwardJointGroupCommandController<HardwareInterface>::preemptActiveGoal()
{
RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);

// Cancel any goal timeout
goal_duration_timer_.stop();

// Cancels the currently active goal
if (current_active_goal)
{
// Marks the current goal as canceled
rt_active_goal_.reset();
current_active_goal->gh_.setCanceled();
}
}

template <class HardwareInterface>
void ForwardJointGroupCommandController<HardwareInterface>::commandCB(const std_msgs::Float64MultiArrayConstPtr& msg)
{
// Preconditions
if (!this->isRunning())
{
ROS_ERROR_STREAM_NAMED(name_, "Can't accept new commands. Controller is not running.");
return;
}

if (!msg)
{
ROS_WARN_STREAM_NAMED(name_, "Received null-pointer message, skipping.");
return;
}

if(msg->data.size()!=n_joints_)
{
ROS_ERROR_STREAM_NAMED(name_, "Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
return;
}

commands_buffer_.writeFromNonRT(msg->data);
preemptActiveGoal();
}

template <class HardwareInterface>
void ForwardJointGroupCommandController<HardwareInterface>::setGoal(GoalHandle gh,
std::vector<double> command)
{
ROS_DEBUG_STREAM_NAMED(name_,"Received new action goal");
pr_control_msgs::JointGroupCommandResult result;

// Preconditions
if (!this->isRunning())
{
result.error_string = "Can't accept new action goals. Controller is not running.";
ROS_ERROR_STREAM_NAMED(name_, result.error_string);
result.error_code = pr_control_msgs::JointGroupCommandResult::INVALID_GOAL;
gh.setRejected(result);
return;
}


if (gh.getGoal()->joint_names.size() != command.size()) {
result.error_string = "Size of command must match size of joint_names.";
ROS_ERROR_STREAM_NAMED(name_, result.error_string);
result.error_code = pr_control_msgs::JointGroupCommandResult::INVALID_GOAL;
gh.setRejected(result);
return;
}

// Goal should specify valid controller joints (they can be ordered differently). Reject if this is not the case
using internal::mapping;
std::vector<unsigned int> mapping_vector = mapping(gh.getGoal()->joint_names, joint_names_);

if (mapping_vector.size() != gh.getGoal()->joint_names.size())
{
result.error_string = "Joints on incoming goal don't match the controller joints.";
ROS_ERROR_STREAM_NAMED(name_, result.error_string);
result.error_code = pr_control_msgs::JointGroupCommandResult::INVALID_JOINTS;
gh.setRejected(result);
return;
}

// update new command
RealtimeGoalHandlePtr rt_goal(new RealtimeGoalHandle(gh));
std::vector< double > new_commands = default_commands_;
for(int i = 0; i < mapping_vector.size(); i++) {
new_commands[mapping_vector[i]] = command[i];
}
rt_goal->preallocated_feedback_->joint_names = joint_names_;
commands_buffer_.writeFromNonRT(new_commands);

// Accept new goal
preemptActiveGoal();
gh.setAccepted();
rt_active_goal_ = rt_goal;

// Setup goal status checking timer
goal_handle_timer_ = controller_nh_.createTimer(action_monitor_period_,
&RealtimeGoalHandle::runNonRealtime,
rt_goal);
goal_handle_timer_.start();

// Setup goal timeout
if (gh.getGoal()->command.time_from_start > ros::Duration()) {
goal_duration_timer_ = controller_nh_.createTimer(gh.getGoal()->command.time_from_start,
&ForwardJointGroupCommandController::timeoutCB,
this,
true);
goal_duration_timer_.start();
}
}

template <class HardwareInterface>
void ForwardJointGroupCommandController<HardwareInterface>::timeoutCB(const ros::TimerEvent& event)
{
RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);

// Check that timeout refers to currently active goal (if any)
if (current_active_goal) {
ROS_DEBUG_NAMED(name_, "Active action goal reached requested timeout.");

// Give sub-classes option to update default_commands_
updateDefaultCommand();
commands_buffer_.writeFromNonRT(default_commands_);

// Marks the current goal as succeeded
rt_active_goal_.reset();
current_active_goal->gh_.setSucceeded();
}
}

template <class HardwareInterface>
void ForwardJointGroupCommandController<HardwareInterface>::cancelCB(GoalHandle gh)
{
RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);

// Check that cancel request refers to currently active goal
if (current_active_goal && current_active_goal->gh_ == gh)
{
ROS_DEBUG_NAMED(name_, "Canceling active action goal because cancel callback recieved from actionlib.");

// Give sub-classes option to update default_commands_
updateDefaultCommand();
commands_buffer_.writeFromNonRT(default_commands_);

preemptActiveGoal();
}
}

template <class HardwareInterface>
void ForwardJointGroupCommandController<HardwareInterface>::setActionFeedback(const ros::Time& time)
{
RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);
if (!current_active_goal)
{
return;
}

current_active_goal->preallocated_feedback_->header.stamp = time;
current_active_goal->preallocated_feedback_->desired = current_active_goal->gh_.getGoal()->command;
current_active_goal->preallocated_feedback_->actual.positions.clear();
current_active_goal->preallocated_feedback_->actual.velocities.clear();
current_active_goal->preallocated_feedback_->actual.effort.clear();
for (auto j : joints_)
{
current_active_goal->preallocated_feedback_->actual.positions.push_back(j.getPosition());
current_active_goal->preallocated_feedback_->actual.velocities.push_back(j.getVelocity());
current_active_goal->preallocated_feedback_->actual.effort.push_back(j.getEffort());
}

current_active_goal->setFeedback( current_active_goal->preallocated_feedback_ );
}

} // namespace
Loading