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