Skip to content
Snippets Groups Projects
Commit e6b011d3 authored by Simon Gabl's avatar Simon Gabl
Browse files

Cleanup the example code.

parent 113dd49a
Branches
Tags
No related merge requests found
......@@ -17,7 +17,6 @@ namespace franka_example_controllers {
class JointPositionExampleController : public controller_interface::MultiInterfaceController<
hardware_interface::PositionJointInterface> {
public:
JointPositionExampleController();
bool init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& /* controller_node_handle */) override;
......
......@@ -11,7 +11,7 @@
<group ns="$(arg arm_id)">
<rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="force_example_controller"/>
<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_description)/rviz/franka_description.rviz"/>
<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/robot.rviz"/>
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
</group>
</launch>
......@@ -17,17 +17,17 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robo
ros::NodeHandle& node_handle) {
std::vector<double> cartesian_stiffness_vector;
std::vector<double> cartesian_damping_vector;
std::string arm_id;
std::vector<std::string> joint_names;
sub_equilibrium_pose_ = node_handle.subscribe(
"/equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
ros::TransportHints().reliable().tcpNoDelay());
std::string arm_id;
if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
return false;
}
std::vector<std::string> joint_names;
if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) {
ROS_ERROR(
"CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
......
......@@ -11,14 +11,12 @@
#include <hardware_interface/hardware_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <xmlrpcpp/XmlRpcValue.h>
namespace franka_example_controllers {
bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_hardware,
ros::NodeHandle& root_node_handle,
ros::NodeHandle& /* controller_node_handle */) {
std::string arm_id;
cartesian_pose_interface_ = robot_hardware->get<franka_hw::FrankaPoseCartesianInterface>();
if (cartesian_pose_interface_ == nullptr) {
ROS_ERROR(
......@@ -27,6 +25,7 @@ bool CartesianPoseExampleController::init(hardware_interface::RobotHW* robot_har
return false;
}
std::string arm_id;
if (!root_node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("CartesianPoseExampleController: Could not get parameter arm_id");
return false;
......
......@@ -11,7 +11,6 @@
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <xmlrpcpp/XmlRpcValue.h>
namespace franka_example_controllers {
......
......@@ -9,7 +9,6 @@
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <xmlrpcpp/XmlRpcValue.h>
namespace franka_example_controllers {
......@@ -22,19 +21,17 @@ bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_har
"JointPositionExampleController: Error getting position joint interface from hardware!");
return false;
}
XmlRpc::XmlRpcValue parameters;
if (!root_node_handle.getParam("joint_names", parameters)) {
std::vector<std::string> joint_names;
if (!root_node_handle.getParam("joint_names", joint_names)) {
ROS_ERROR("JointPositionExampleController: Could not parse joint names");
}
if (parameters.size() != 7) {
if (joint_names.size() != 7) {
ROS_ERROR_STREAM("JointPositionExampleController: Wrong number of joint names, got "
<< int(parameters.size()) << " instead of 7 names!");
<< joint_names.size() << " instead of 7 names!");
return false;
}
position_joint_handles_.resize(7);
std::array<std::string, 7> joint_names;
for (size_t i = 0; i < 7; ++i) {
joint_names[i] = static_cast<std::string>(parameters[i]);
try {
position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names[i]);
} catch (const hardware_interface::HardwareInterfaceException& e) {
......
......@@ -9,7 +9,6 @@
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <xmlrpcpp/XmlRpcValue.h>
namespace franka_example_controllers {
......@@ -22,19 +21,17 @@ bool JointVelocityExampleController::init(hardware_interface::RobotHW* robot_har
"JointVelocityExampleController: Error getting velocity joint interface from hardware!");
return false;
}
XmlRpc::XmlRpcValue parameters;
if (!root_node_handle.getParam("joint_names", parameters)) {
std::vector<std::string> joint_names;
if (!root_node_handle.getParam("joint_names", joint_names)) {
ROS_ERROR("JointVelocityExampleController: Could not parse joint names");
}
if (parameters.size() != 7) {
if (joint_names.size() != 7) {
ROS_ERROR_STREAM("JointVelocityExampleController: Wrong number of joint names, got "
<< int(parameters.size()) << " instead of 7 names!");
<< joint_names.size() << " instead of 7 names!");
return false;
}
velocity_joint_handles_.resize(7);
std::array<std::string, 7> joint_names;
for (size_t i = 0; i < 7; ++i) {
joint_names[i] = static_cast<std::string>(parameters[i]);
try {
velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names[i]);
} catch (const hardware_interface::HardwareInterfaceException& ex) {
......
......@@ -31,12 +31,12 @@ namespace franka_example_controllers {
bool ModelExampleController::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& node_handle) {
std::string arm_id;
franka_state_interface_ = robot_hw->get<franka_hw::FrankaStateInterface>();
if (franka_state_interface_ == nullptr) {
ROS_ERROR("ModelExampleController: Could not get Franka state interface from hardware");
return false;
}
std::string arm_id;
if (!node_handle.getParam("arm_id", arm_id)) {
ROS_ERROR("ModelExampleController: Could not read parameter arm_id");
return false;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment