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

Merge remote-tracking branch 'origin/develop'.

parents 01988195 4c98d7ab
No related branches found
No related tags found
No related merge requests found
...@@ -8,6 +8,7 @@ ...@@ -8,6 +8,7 @@
to the robot state to the robot state
* Updated and improved examples. * Updated and improved examples.
* Fix includes for Eigen3 in `franka_example_controllers` * Fix includes for Eigen3 in `franka_example_controllers`
* Fixed gripper state publishing in `franka_gripper_node`
## 0.1.2 - 2017-10-10 ## 0.1.2 - 2017-10-10
......
...@@ -11,7 +11,6 @@ ...@@ -11,7 +11,6 @@
#include <ros/rate.h> #include <ros/rate.h>
#include <ros/spinner.h> #include <ros/spinner.h>
#include <sensor_msgs/JointState.h> #include <sensor_msgs/JointState.h>
#include <xmlrpcpp/XmlRpc.h>
#include <franka/gripper_state.h> #include <franka/gripper_state.h>
#include <franka_gripper/franka_gripper.h> #include <franka_gripper/franka_gripper.h>
...@@ -133,28 +132,27 @@ int main(int argc, char** argv) { ...@@ -133,28 +132,27 @@ int main(int argc, char** argv) {
<< publish_rate); << publish_rate);
} }
XmlRpc::XmlRpcValue params; std::vector<std::string> joint_names;
if (!node_handle.getParam("joint_names", params)) { if (!node_handle.getParam("joint_names", joint_names)) {
ROS_ERROR("franka_gripper_node: Could not parse joint_names!"); ROS_ERROR("franka_gripper_node: Could not parse joint_names!");
return -1; return -1;
} }
if (params.size() != 2) { if (joint_names.size() != 2) {
ROS_ERROR("franka_gripper_node: Got wrong number of joint_names!"); ROS_ERROR("franka_gripper_node: Got wrong number of joint_names!");
return -1; return -1;
} }
std::vector<std::string> joint_names(params.size());
for (int i = 0; i < params.size(); ++i) {
joint_names[i] = static_cast<std::string>(params[i]);
}
franka::GripperState gripper_state; franka::GripperState gripper_state;
std::mutex gripper_state_mutex; std::mutex gripper_state_mutex;
std::thread read_thread([&gripper_state, &gripper, &gripper_state_mutex]() { std::thread read_thread([&gripper_state, &gripper, &gripper_state_mutex]() {
ros::Rate read_rate(10); ros::Rate read_rate(10);
while (read_rate.sleep() && ros::ok()) { while (ros::ok()) {
std::lock_guard<std::mutex> lock(gripper_state_mutex); {
std::lock_guard<std::mutex> _(gripper_state_mutex);
updateGripperState(gripper, &gripper_state); updateGripperState(gripper, &gripper_state);
} }
read_rate.sleep();
}
}); });
ros::Publisher gripper_state_publisher = ros::Publisher gripper_state_publisher =
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment