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 a7c877d175337306cd61f4fe3830b6107310b04c..daf4c4d81cb7d66ee7f4412d4202e4839452fa9f 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 0eee3686ffccc5be71e5633cd6eea0f76142e1f3..0187bbda9c8e5f501cab2865a6a4f69a9e73e652 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 5c6d8ebe00443983cf9bac330e662740680fb4e9..5b275fbaa1fe3caaf0ef17748194f8ab6c36f4d0 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 6417b3e335d3297b5cae7f1ed0708ab685df34ea..223de376e235574bb01d7a542f7ae7edc2e8bfc7 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 afd91ff268905090faa155fc32dcabd00821a352..3bc8f80f2c9c169f54537930eb55a49aab5a2f39 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 fed70ce2253b6d50d2deee8d4d6640099021f90a..847da23de598711bbc607d68531e23a7d5b35a75 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 8da0f487d678e914c607be54670f0e29fd16f49e..bcc3ca286d79373e6a74bcd393b1f7f331b7049a 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 67ddf05f9415b428ac431e0cbe7a6e8eb6e3034a..8d54318415ffbb56b7c9f1f0f472c4de7f0496ae 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;