diff --git a/CHANGELOG.md b/CHANGELOG.md index 34cbc30e7a2f7321d1aacbeb23f0ef0505817e1c..e132d8255f46002d1e6682aa01282dba90e51bd4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ Requires `libfranka` >= 0.3.0 * Added missing dependency to `panda_moveit_config` * Fixed linker errors when building with `-DFranka_DIR` while an older version of `ros-kinetic-libfranka` is installed + * Added gripper joint state publisher to `franka_visualization` ## 0.2.2 - 2018-01-31 diff --git a/franka_visualization/CMakeLists.txt b/franka_visualization/CMakeLists.txt index 7111e3fc07413704cf27ab59bd60ed58d1e82907..ab9e967f2f42ef79ab16ebd176d42451a270c414 100644 --- a/franka_visualization/CMakeLists.txt +++ b/franka_visualization/CMakeLists.txt @@ -13,24 +13,28 @@ find_package(Franka 0.3.0 REQUIRED) catkin_package(CATKIN_DEPENDS sensor_msgs roscpp) -add_executable(franka_joint_state_publisher - src/franka_joint_state_publisher.cpp -) -add_dependencies(franka_joint_state_publisher - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_include_directories(franka_joint_state_publisher SYSTEM PUBLIC - ${Franka_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) -target_link_libraries(franka_joint_state_publisher PUBLIC - ${Franka_LIBRARIES} - ${catkin_LIBRARIES} -) +set(EXECUTABLES robot_joint_state_publisher gripper_joint_state_publisher) + +foreach(executable ${EXECUTABLES}) + add_executable(${executable} + src/${executable}.cpp + ) + add_dependencies(${executable} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(${executable} SYSTEM PUBLIC + ${Franka_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ) + target_link_libraries(${executable} PUBLIC + ${Franka_LIBRARIES} + ${catkin_LIBRARIES} + ) +endforeach() ## Installation -install(TARGETS franka_joint_state_publisher +install(TARGETS ${EXECUTABLES} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -55,6 +59,6 @@ if(CLANG_TOOLS) add_format_target(franka_visualization FILES ${SOURCES} ${HEADERS}) add_tidy_target(franka_visualization FILES ${SOURCES} - DEPENDS franka_joint_state_publisher + DEPENDS ${EXECUTABLES} ) endif() diff --git a/franka_visualization/config/gripper_settings.yaml b/franka_visualization/config/gripper_settings.yaml new file mode 100644 index 0000000000000000000000000000000000000000..0a286b2626aa21ede73342b5bcb129ddb9af1750 --- /dev/null +++ b/franka_visualization/config/gripper_settings.yaml @@ -0,0 +1,3 @@ +joint_names: + - panda_finger_joint1 + - panda_finger_joint2 diff --git a/franka_visualization/config/robot_read_settings.yaml b/franka_visualization/config/robot_settings.yaml similarity index 100% rename from franka_visualization/config/robot_read_settings.yaml rename to franka_visualization/config/robot_settings.yaml diff --git a/franka_visualization/launch/franka_visualization.launch b/franka_visualization/launch/franka_visualization.launch index 6bad56141c088e6fed5b2ffb1f184c32ca602a93..92890a718039467d8ceada0db519ced0c01074ba 100644 --- a/franka_visualization/launch/franka_visualization.launch +++ b/franka_visualization/launch/franka_visualization.launch @@ -3,20 +3,28 @@ <launch> <arg name="load_gripper" default="true" /> <arg name="robot_ip" default="robot.franka.de" /> + <arg name="publish_rate" default="30" /> <param unless="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm.urdf.xacro" /> <param if="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm_hand.urdf.xacro" /> - <node name="franka_joint_state_publisher" pkg="franka_visualization" type="franka_joint_state_publisher" output="screen"> - <rosparam command="load" file="$(find franka_visualization)/config/robot_read_settings.yaml" /> + <node name="robot_joint_state_publisher" pkg="franka_visualization" type="robot_joint_state_publisher" output="screen"> + <rosparam command="load" file="$(find franka_visualization)/config/robot_settings.yaml" /> <param name="robot_ip" value="$(arg robot_ip)" /> + <param name="publish_rate" value="$(arg publish_rate)" /> + </node> + + <node name="gripper_joint_state_publisher" pkg="franka_visualization" type="gripper_joint_state_publisher" output="screen" if="$(arg load_gripper)"> + <rosparam command="load" file="$(find franka_visualization)/config/gripper_settings.yaml" /> + <param name="robot_ip" value="$(arg robot_ip)" /> + <param name="publish_rate" value="$(arg publish_rate)" /> </node> <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen"> <param if="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" /> <param unless="$(arg load_gripper)" name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" /> - <rosparam param="source_list">[franka_joint_state_publisher/joint_states] </rosparam> - <param if="$(arg load_gripper)" name="publish_default_positions" value="true" /> + <param name="rate" value="$(arg publish_rate)" /> + <rosparam param="source_list">[franka_joint_state_publisher/joint_states, franka_gripper_state_publisher/joint_states]</rosparam> </node> <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> diff --git a/franka_visualization/src/franka_joint_state_publisher.cpp b/franka_visualization/src/franka_joint_state_publisher.cpp deleted file mode 100644 index 8763cebc9b285a805d989a712b75acbc69f42cf7..0000000000000000000000000000000000000000 --- a/franka_visualization/src/franka_joint_state_publisher.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright (c) 2017 Franka Emika GmbH -// Use of this source code is governed by the Apache-2.0 license, see LICENSE -#include <franka/exception.h> -#include <franka/robot.h> -#include <ros/ros.h> -#include <sensor_msgs/JointState.h> - -int main(int argc, char** argv) { - ros::init(argc, argv, "franka_joint_state_publisher"); - ros::NodeHandle private_nodehandle("~"); - ros::Rate rate(30.0); - ros::Publisher joint_pub = - - private_nodehandle.advertise<sensor_msgs::JointState>("joint_states", 1); - const size_t kNumberOfJoints = 7; - std::array<std::string, kNumberOfJoints> joint_names; - XmlRpc::XmlRpcValue params; - private_nodehandle.getParam("joint_names", params); - - for (size_t i = 0; i < kNumberOfJoints; i++) { - joint_names[i] = static_cast<std::string>(params[i]); - } - - std::string robot_ip; - private_nodehandle.getParam("robot_ip", robot_ip); - - sensor_msgs::JointState states; - states.effort.resize(kNumberOfJoints); - states.name.resize(kNumberOfJoints); - states.position.resize(kNumberOfJoints); - states.velocity.resize(kNumberOfJoints); - - for (size_t i = 0; i < joint_names.size(); i++) { - states.name[i] = joint_names[i]; - } - - try { - franka::Robot robot(robot_ip); - - uint32_t sequence_number = 0; - robot.read([&](const franka::RobotState& robot_state) { - states.header.stamp = ros::Time::now(); - states.header.seq = sequence_number++; - for (size_t i = 0; i < kNumberOfJoints; i++) { - states.position[i] = robot_state.q[i]; - states.velocity[i] = robot_state.dq[i]; - states.effort[i] = robot_state.tau_J[i]; - } - joint_pub.publish(states); - ros::spinOnce(); - rate.sleep(); - return ros::ok(); - }); - - } catch (const franka::Exception& e) { - ROS_ERROR_STREAM("" << e.what()); - return -1; - } - - return 0; -} diff --git a/franka_visualization/src/gripper_joint_state_publisher.cpp b/franka_visualization/src/gripper_joint_state_publisher.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c689a22b1aceb9ae7414146066489334b81f2ae6 --- /dev/null +++ b/franka_visualization/src/gripper_joint_state_publisher.cpp @@ -0,0 +1,57 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include <franka/exception.h> +#include <franka/gripper.h> +#include <ros/ros.h> +#include <sensor_msgs/JointState.h> + +int main(int argc, char** argv) { + ros::init(argc, argv, "gripper_joint_state_publisher"); + ros::NodeHandle node_handle("~"); + + std::vector<std::string> joint_names; + node_handle.getParam("joint_names", joint_names); + + std::string robot_ip; + node_handle.getParam("robot_ip", robot_ip); + + double publish_rate; + node_handle.getParam("publish_rate", publish_rate); + + ros::Rate rate(publish_rate); + + sensor_msgs::JointState states; + states.effort.resize(joint_names.size()); + states.name.resize(joint_names.size()); + states.position.resize(joint_names.size()); + states.velocity.resize(joint_names.size()); + + for (size_t i = 0; i < joint_names.size(); i++) { + states.name[i] = joint_names[i]; + } + + ros::Publisher publisher = node_handle.advertise<sensor_msgs::JointState>("joint_states", 1); + + try { + franka::Gripper gripper(robot_ip); + + while (ros::ok()) { + franka::GripperState gripper_state = gripper.readOnce(); + states.header.stamp = ros::Time::now(); + for (size_t i = 0; i < joint_names.size(); i++) { + states.position[i] = gripper_state.width * 0.5; + states.velocity[i] = 0.0; + states.effort[i] = 0.0; + } + publisher.publish(states); + ros::spinOnce(); + rate.sleep(); + } + + } catch (const franka::Exception& e) { + ROS_ERROR_STREAM("Exception: " << e.what()); + return -1; + } + + return 0; +} diff --git a/franka_visualization/src/robot_joint_state_publisher.cpp b/franka_visualization/src/robot_joint_state_publisher.cpp new file mode 100644 index 0000000000000000000000000000000000000000..61fed6c7ef97b36c870ab97aff0c71de68b6bcad --- /dev/null +++ b/franka_visualization/src/robot_joint_state_publisher.cpp @@ -0,0 +1,57 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include <franka/exception.h> +#include <franka/robot.h> +#include <ros/ros.h> +#include <sensor_msgs/JointState.h> + +int main(int argc, char** argv) { + ros::init(argc, argv, "robot_joint_state_publisher"); + ros::NodeHandle node_handle("~"); + + std::vector<std::string> joint_names; + node_handle.getParam("joint_names", joint_names); + + std::string robot_ip; + node_handle.getParam("robot_ip", robot_ip); + + double publish_rate; + node_handle.getParam("publish_rate", publish_rate); + + ros::Rate rate(publish_rate); + + sensor_msgs::JointState states; + states.effort.resize(joint_names.size()); + states.name.resize(joint_names.size()); + states.position.resize(joint_names.size()); + states.velocity.resize(joint_names.size()); + + for (size_t i = 0; i < joint_names.size(); i++) { + states.name[i] = joint_names[i]; + } + + ros::Publisher publisher = node_handle.advertise<sensor_msgs::JointState>("joint_states", 1); + + try { + franka::Robot robot(robot_ip); + + robot.read([&](const franka::RobotState& robot_state) { + states.header.stamp = ros::Time::now(); + for (size_t i = 0; i < joint_names.size(); i++) { + states.position[i] = robot_state.q[i]; + states.velocity[i] = robot_state.dq[i]; + states.effort[i] = robot_state.tau_J[i]; + } + publisher.publish(states); + ros::spinOnce(); + rate.sleep(); + return ros::ok(); + }); + + } catch (const franka::Exception& e) { + ROS_ERROR_STREAM("Exception: " << e.what()); + return -1; + } + + return 0; +}