From e6b011d3a8da6ee215a537256958b5058cabd562 Mon Sep 17 00:00:00 2001
From: Simon Gabl <simon.gabl@franka.de>
Date: Wed, 24 Jan 2018 16:50:11 +0100
Subject: [PATCH] Cleanup the example code.

---
 .../joint_position_example_controller.h               |  1 -
 .../launch/force_example_controller.launch            |  2 +-
 .../src/cartesian_impedance_example_controller.cpp    |  4 ++--
 .../src/cartesian_pose_example_controller.cpp         |  3 +--
 .../src/cartesian_velocity_example_controller.cpp     |  1 -
 .../src/joint_position_example_controller.cpp         | 11 ++++-------
 .../src/joint_velocity_example_controller.cpp         | 11 ++++-------
 .../src/model_example_controller.cpp                  |  2 +-
 8 files changed, 13 insertions(+), 22 deletions(-)

diff --git a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
index a7c877d..daf4c4d 100644
--- a/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/joint_position_example_controller.h
@@ -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;
diff --git a/franka_example_controllers/launch/force_example_controller.launch b/franka_example_controllers/launch/force_example_controller.launch
index 0eee368..0187bbd 100644
--- a/franka_example_controllers/launch/force_example_controller.launch
+++ b/franka_example_controllers/launch/force_example_controller.launch
@@ -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>
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
index 5c6d8eb..5b275fb 100644
--- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
@@ -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, "
diff --git a/franka_example_controllers/src/cartesian_pose_example_controller.cpp b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
index 6417b3e..223de37 100644
--- a/franka_example_controllers/src/cartesian_pose_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_pose_example_controller.cpp
@@ -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;
diff --git a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
index afd91ff..3bc8f80 100644
--- a/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_velocity_example_controller.cpp
@@ -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 {
 
diff --git a/franka_example_controllers/src/joint_position_example_controller.cpp b/franka_example_controllers/src/joint_position_example_controller.cpp
index fed70ce..847da23 100644
--- a/franka_example_controllers/src/joint_position_example_controller.cpp
+++ b/franka_example_controllers/src/joint_position_example_controller.cpp
@@ -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) {
diff --git a/franka_example_controllers/src/joint_velocity_example_controller.cpp b/franka_example_controllers/src/joint_velocity_example_controller.cpp
index 8da0f48..bcc3ca2 100644
--- a/franka_example_controllers/src/joint_velocity_example_controller.cpp
+++ b/franka_example_controllers/src/joint_velocity_example_controller.cpp
@@ -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) {
diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp
index 67ddf05..8d54318 100644
--- a/franka_example_controllers/src/model_example_controller.cpp
+++ b/franka_example_controllers/src/model_example_controller.cpp
@@ -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;
-- 
GitLab