Commit 2d640ca8 authored by Simon M. Haller-Seeber's avatar Simon M. Haller-Seeber
Browse files

Merge branch 'master' of https://git.uibk.ac.at/c703100/minibot

parents 40fc5fe5 5b39aeb2
cmake_minimum_required(VERSION 2.8.3)
project(lss_4dof_hw)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++17 -fconcepts)
# Project root directory
GET_FILENAME_COMPONENT(PROJECT_ROOT . ABSOLUTE CACHE INTERNAL "Path prefix for the project")
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
cmake_modules
hardware_interface
controller_manager
roscpp
control_msgs
trajectory_msgs
actionlib
urdf
joint_limits_interface
transmission_interface
control_toolbox
std_msgs
sensor_msgs
rosparam_shortcuts
roslib
geometry_msgs
rospy
message_generation
serial
tf
)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
catkin_package(
# INCLUDE_DIRS include
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime serial
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
${catkin_INCLUDE_DIRS}
)
# Hardware Interface
add_library(lss_4dof_hw_interface
${PROJECT_ROOT}/src/lss_4dof_hw_interface.cpp
)
add_executable(lss_4dof_hw_node ${PROJECT_ROOT}/src/lss_4dof_hw_node.cpp ${PROJECT_ROOT}/src/lss_4dof_hw_interface.cpp ${PROJECT_ROOT}/src/lss_4dof_hw_interface.h)
target_link_libraries(lss_4dof_hw_node lss_4dof_hw_interface ${catkin_LIBRARIES})
#############
## Install ##
#############
install(TARGETS
lss_4dof_hw_interface
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
# Install executables
install(TARGETS
lss_4dof_hw_node
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_rvr_ros.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
<?xml version="1.0"?>
<package format="2">
<name>lss_4dof_hw</name>
<version>0.0.1</version>
<description>The LSS 4 DOF ROS hardware package. </description>
<maintainer email="simon.haller-seeber@uibk.ac.at">Simon M. Haller-Seeber</maintainer>
<license>GPLv3</license>
<author email="no-email@no-domain.com">Rud Merriam</author>
<author email="simon.haller-seeber@uibk.ac.at">Simon M. Haller-Seeber</author>
<depend>roscpp</depend>
<depend>rospy</depend>
<depend>std_msgs</depend>
<depend>serial</depend>
<depend>tf</depend>
<build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<buildtool_depend>catkin</buildtool_depend>
<exec_depend>message_runtime</exec_depend>
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
// ROS
#include <limits>
#include <rosparam_shortcuts/rosparam_shortcuts.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32MultiArray.h"
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "lss_4dof_hw_interface.h"
#include <iostream> // std::cout, std::endl
# define M_PI 3.14159265358979323846 /* pi */
using namespace std::literals;
namespace lss_4dof_hw_interface
{
// Connect to LSS Arm
//SerialPort serial_port { "/dev/ttyUSB0", 115200 };
LSS4DOFHWInterface::LSS4DOFHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model) :
name_("lss_4dof_hw_interface"),
nh_(nh),
use_rosparam_joint_limits_(false),
use_soft_limits_if_available_(false)
{
// Check if the URDF model needs to be loaded
if (urdf_model == NULL)
loadURDF(nh, "/robot_description");
else
urdf_model_ = urdf_model;
// Load rosparams
ros::NodeHandle rpnh(nh_, "hardware_interface");
std::size_t error = 0;
error += !rosparam_shortcuts::get("hardware_interface", rpnh, "joints", joint_names_);
rosparam_shortcuts::shutdownIfError("hardware_interface", error);
}
void LSS4DOFHWInterface::init()
{
ROS_INFO("LSS 4 DOF ready");
num_joints_ = joint_names_.size();
// Status
joint_position_.resize(num_joints_, 0.0);
joint_velocity_.resize(num_joints_, 0.0);
joint_effort_.resize(num_joints_, 0.0);
// Command
joint_position_command_.resize(num_joints_, 0.0);
joint_velocity_command_.resize(num_joints_, 0.0);
joint_effort_command_.resize(num_joints_, 0.0);
// Limits
joint_position_lower_limits_.resize(num_joints_, 0.0);
joint_position_upper_limits_.resize(num_joints_, 0.0);
joint_velocity_limits_.resize(num_joints_, 0.0);
joint_effort_limits_.resize(num_joints_, 0.0);
// Initialize interfaces for each joint
for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id)
{
ROS_DEBUG_STREAM_NAMED(name_, "Loading joint name: " << joint_names_[joint_id]);
// Create joint state interface
joint_state_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[joint_id], &joint_position_[joint_id], &joint_velocity_[joint_id], &joint_effort_[joint_id]));
// Add command interfaces to joints
// TODO: decide based on transmissions?
hardware_interface::JointHandle joint_handle_position = hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_position_command_[joint_id]);
position_joint_interface_.registerHandle(joint_handle_position);
hardware_interface::JointHandle joint_handle_velocity = hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_velocity_command_[joint_id]);
velocity_joint_interface_.registerHandle(joint_handle_velocity);
hardware_interface::JointHandle joint_handle_effort = hardware_interface::JointHandle(joint_state_interface_.getHandle(joint_names_[joint_id]), &joint_effort_command_[joint_id]);
effort_joint_interface_.registerHandle(joint_handle_effort);
// Load the joint limits
registerJointLimits(joint_handle_position, joint_handle_velocity, joint_handle_effort, joint_id);
} // end for each joint
registerInterface(&joint_state_interface_); // From RobotHW base class.
registerInterface(&position_joint_interface_); // From RobotHW base class.
registerInterface(&velocity_joint_interface_); // From RobotHW base class.
registerInterface(&effort_joint_interface_); // From RobotHW base class.
ROS_INFO_STREAM_NAMED(name_, "Sphero RVR Hardware Interface Ready.");
}
void LSS4DOFHWInterface::read(ros::Duration &elapsed_time)
{
// ToDo current position from Arm
//joint_position_.clear();
//joint_position_.push_back();
//joint_position_.push_back(v_.y);
}
void LSS4DOFHWInterface::write(ros::Duration &elapsed_time)
{
// Safety
ROS_INFO_STREAM_NAMED(name_, "Update L:" << joint_velocity_command_.at(0) << " R:" << joint_velocity_command_.at(1) );
enforceLimits(elapsed_time);
// ToDo Write Position to Motors
}
void LSS4DOFHWInterface::enforceLimits(ros::Duration &period)
{
// Saturation limits
pos_jnt_sat_interface_.enforceLimits(period);
//vel_jnt_sat_interface_.enforceLimits(period);
// eff_jnt_sat_interface_.enforceLimits(period);
// Soft limits
pos_jnt_soft_limits_.enforceLimits(period);
//vel_jnt_soft_limits_.enforceLimits(period);
// eff_jnt_soft_limits_.enforceLimits(period);
}
void LSS4DOFHWInterface::registerJointLimits(const hardware_interface::JointHandle &joint_handle_position, const hardware_interface::JointHandle &joint_handle_velocity, const hardware_interface::JointHandle &joint_handle_effort, std::size_t joint_id)
{
// Default values
joint_position_lower_limits_[joint_id] = -std::numeric_limits<double>::max();
joint_position_upper_limits_[joint_id] = std::numeric_limits<double>::max();
joint_velocity_limits_[joint_id] = std::numeric_limits<double>::max();
joint_effort_limits_[joint_id] = std::numeric_limits<double>::max();
// Limits datastructures
joint_limits_interface::JointLimits joint_limits; // Position
joint_limits_interface::SoftJointLimits soft_limits; // Soft Position
bool has_joint_limits = false;
bool has_soft_limits = false;
// Get limits from URDF
if (urdf_model_ == NULL)
{
ROS_WARN_STREAM_NAMED(name_, "No URDF model loaded, unable to get joint limits");
return;
}
// Get limits from URDF
urdf::JointConstSharedPtr urdf_joint = urdf_model_->getJoint(joint_names_[joint_id]);
// Get main joint limits
if (urdf_joint == NULL)
{
ROS_ERROR_STREAM_NAMED(name_, "URDF joint not found " << joint_names_[joint_id]);
return;
}
// Get limits from URDF
if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits))
{
has_joint_limits = true;
ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has URDF position limits [" << joint_limits.min_position << ", " << joint_limits.max_position << "]");
if (joint_limits.has_velocity_limits)
ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has URDF velocity limit [" << joint_limits.max_velocity << "]");
}
else
{
if (urdf_joint->type != urdf::Joint::CONTINUOUS)
ROS_WARN_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " does not have a URDF position limit");
}
// Get limits from ROS param
if (use_rosparam_joint_limits_)
{
if (joint_limits_interface::getJointLimits(joint_names_[joint_id], nh_, joint_limits))
{
has_joint_limits = true;
ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has rosparam position limits [" << joint_limits.min_position << ", " << joint_limits.max_position << "]");
if (joint_limits.has_velocity_limits)
ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has rosparam velocity limit [" << joint_limits.max_velocity << "]");
} // the else debug message provided internally by joint_limits_interface
}
// Get soft limits from URDF
if (use_soft_limits_if_available_)
{
if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits))
{
has_soft_limits = true;
ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " has soft joint limits.");
}
else
{
ROS_DEBUG_STREAM_NAMED(name_, "Joint " << joint_names_[joint_id] << " does not have soft joint limits");
}
}
// Quit we we haven't found any limits in URDF or rosparam server
if (!has_joint_limits)
{
return;
}
// Copy position limits if available
if (joint_limits.has_position_limits)
{
// Slighly reduce the joint limits to prevent floating point errors
joint_limits.min_position += std::numeric_limits<double>::epsilon();
joint_limits.max_position -= std::numeric_limits<double>::epsilon();
joint_position_lower_limits_[joint_id] = joint_limits.min_position;
joint_position_upper_limits_[joint_id] = joint_limits.max_position;
}
// Copy velocity limits if available
if (joint_limits.has_velocity_limits)
{
joint_velocity_limits_[joint_id] = joint_limits.max_velocity;
}
// Copy effort limits if available
if (joint_limits.has_effort_limits)
{
joint_effort_limits_[joint_id] = joint_limits.max_effort;
}
if (has_soft_limits) // Use soft limits
{
ROS_DEBUG_STREAM_NAMED(name_, "Using soft saturation limits");
const joint_limits_interface::PositionJointSoftLimitsHandle soft_handle_position(joint_handle_position, joint_limits, soft_limits);
pos_jnt_soft_limits_.registerHandle(soft_handle_position);
const joint_limits_interface::VelocityJointSoftLimitsHandle soft_handle_velocity(joint_handle_velocity, joint_limits, soft_limits);
vel_jnt_soft_limits_.registerHandle(soft_handle_velocity);
const joint_limits_interface::EffortJointSoftLimitsHandle soft_handle_effort(joint_handle_effort, joint_limits, soft_limits);
eff_jnt_soft_limits_.registerHandle(soft_handle_effort);
}
else // Use saturation limits
{
ROS_DEBUG_STREAM_NAMED(name_, "Using saturation limits (not soft limits)");
const joint_limits_interface::PositionJointSaturationHandle sat_handle_position(joint_handle_position, joint_limits);
pos_jnt_sat_interface_.registerHandle(sat_handle_position);
const joint_limits_interface::VelocityJointSaturationHandle sat_handle_velocity(joint_handle_velocity, joint_limits);
vel_jnt_sat_interface_.registerHandle(sat_handle_velocity);
const joint_limits_interface::EffortJointSaturationHandle sat_handle_effort(joint_handle_effort, joint_limits);
eff_jnt_sat_interface_.registerHandle(sat_handle_effort);
}
}
void LSS4DOFHWInterface::reset()
{
// Reset joint limits state, in case of mode switch or e-stop
//pos_jnt_sat_interface_.reset();
//pos_jnt_soft_limits_.reset();
//vel_jnt_sat_interface_.reset();
//vel_jnt_soft_limits_.reset();
}
void LSS4DOFHWInterface::printState()
{
// WARNING: THIS IS NOT REALTIME SAFE
// FOR DEBUGGING ONLY, USE AT YOUR OWN ROBOT's RISK!
ROS_INFO_STREAM_THROTTLE(1, std::endl << printStateHelper());
}
std::string LSS4DOFHWInterface::printStateHelper()
{
std::stringstream ss;
std::cout.precision(15);
for (std::size_t i = 0; i < num_joints_; ++i)
{
ss << "j" << i << ": " << std::fixed << joint_position_[i] << "\t ";
ss << std::fixed << joint_velocity_[i] << "\t ";
ss << std::fixed << joint_effort_[i] << std::endl;
}
return ss.str();
}
std::string LSS4DOFHWInterface::printCommandHelper()
{
std::stringstream ss;
std::cout.precision(15);
ss << " position velocity effort \n";
for (std::size_t i = 0; i < num_joints_; ++i)
{
ss << "j" << i << ": " << std::fixed << joint_position_command_[i] << "\t ";
ss << std::fixed << joint_velocity_command_[i] << "\t ";
ss << std::fixed << joint_effort_command_[i] << std::endl;
}
return ss.str();
}
void LSS4DOFHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
{
std::string urdf_string;
urdf_model_ = new urdf::Model();
// search and wait for robot_description on param server
while (urdf_string.empty() && ros::ok())
{
std::string search_param_name;
if (nh.searchParam(param_name, search_param_name))
{
ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " << search_param_name);
nh.getParam(search_param_name, urdf_string);
}
else
{
ROS_INFO_STREAM_NAMED(name_, "Waiting for model URDF on the ROS param server at location: " << param_name);
nh.getParam(param_name, urdf_string);
}
usleep(100000);
}
if (!urdf_model_->initString(urdf_string))
ROS_ERROR_STREAM_NAMED(name_, "Unable to load URDF model");
else
ROS_DEBUG_STREAM_NAMED(name_, "Received URDF from param server");
}
void LSS4DOFHWInterface::disconnect()
{
}
}
#ifndef ROS_CONTROL_LSS_4DOF_HW_INTERFACE_H
#define ROS_CONTROL_LSS_4DOF_HW_INTERFACE_H
// C++
#include <boost/scoped_ptr.hpp>
#include <iostream>
#include <cstdlib>
//#include <future>
//#include <thread>
// ROS
#include <ros/ros.h>
#include <urdf/model.h>
// ROS Control
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <controller_manager/controller_manager.h>
namespace lss_4dof_hw_interface
{
class LSS4DOFHWInterface : public hardware_interface::RobotHW
{
public:
/**
* \brief Constructor
* \param nh - Node handle for topics.
* \param urdf - optional pointer to a parsed robot model
*/
LSS4DOFHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model = NULL);
/** \brief Destructor */
virtual ~LSS4DOFHWInterface() {}
/** \brief Initialize the hardware interface */
virtual void init();
/** \brief Read the state from the robot hardware. */
virtual void read(ros::Duration &elapsed_time);
/** \brief Write the command to the robot hardware. */
virtual void write(ros::Duration &elapsed_time);
/** \brief Set all members to default values */
virtual void reset();
/** \brief Set all members to default values */
virtual void disconnect();
/**
* \brief Check (in non-realtime) if given controllers could be started and stopped from the
* current state of the RobotHW
* with regard to necessary hardware interface switches. Start and stop list are disjoint.
* This is just a check, the actual switch is done in doSwitch()
*/
virtual bool canSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list) const
{
return true;
}
/**
* \brief Perform (in non-realtime) all necessary hardware interface switches in order to start
* and stop the given controllers.
* Start and stop list are disjoint. The feasability was checked in canSwitch() beforehand.
*/
virtual void doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list)
{
}
/**
* \brief Register the limits of the joint specified by joint_id and joint_handle. The limits
* are retrieved from the urdf_model.
*
* \return the joint's type, lower position limit, upper position limit, and effort limit.
*/
virtual void registerJointLimits(const hardware_interface::JointHandle &joint_handle_position, const hardware_interface::JointHandle &joint_handle_velocity, const hardware_interface::JointHandle &joint_handle_effort, std::size_t joint_id);
/** \breif Enforce limits for all values before writing */
virtual void enforceLimits(ros::Duration &period);
/** \brief Helper for debugging a joint's state */
virtual void printState();
std::string printStateHelper();
/** \brief Helper for debugging a joint's command */
std::string printCommandHelper();
protected:
/** \brief Sphero RVR interface */
LSS4DOFHWInterface *lss_4dof;
/** \brief Get the URDF XML from the parameter server */
virtual void loadURDF(ros::NodeHandle &nh, std::string param_name);
// Short name of this class
std::string name_;
// Startup and shutdown of the internal node inside a roscpp program
ros::NodeHandle nh_;
// Hardware interfaces
hardware_interface::JointStateInterface joint_state_interface_;
hardware_interface::PositionJointInterface position_joint_interface_;
hardware_interface::VelocityJointInterface velocity_joint_interface_;
hardware_interface::EffortJointInterface effort_joint_interface_;
// Joint limits interfaces - Saturation
joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_;
joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_;
joint_limits_interface::EffortJointSaturationInterface eff_jnt_sat_interface_;
// Joint limits interfaces - Soft limits
joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_limits_;
joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_limits_;
joint_limits_interface::EffortJointSoftLimitsInterface eff_jnt_soft_limits_;
// Configuration
std::vector<std::string> joint_names_;
std::size_t num_joints_;