This is an example to show how to get data from the hand, read the position for a specific joint and send this as the target to another joint. More...
#include <ros/ros.h>
#include <string>
#include <std_msgs/Float64.h>
#include <sr_robot_msgs/JointControllerState.h>
#include <pr2_controllers_msgs/JointControllerState.h>
Go to the source code of this file.
Functions | |
void | callback (const sr_robot_msgs::JointControllerStateConstPtr &msg) |
int | main (int argc, char **argv) |
Variables | |
std::string | child_name = "mfj3" |
the name of the child joint to link to the parent | |
std::string | controller_type = "_mixed_position_velocity_controller" |
the type of controller that will be running | |
std::string | parent_name = "ffj3" |
the name of the parent joint | |
ros::Publisher | pub |
ros::Subscriber | sub |
This is an example to show how to get data from the hand, read the position for a specific joint and send this as the target to another joint.
Copyright 2011 Shadow Robot Company Ltd.
This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 2 of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>.
Copyright 2011 Shadow Robot Company Ltd.
This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 2 of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>.
This example can be tested with the simulated hand (or simulated hand and arm) by using the following commands in separate terminals *
* roslaunch sr_hand gazebo_arm_and_hand.launch * or * roslaunch sr_hand gazebo_hand.launch * and then * rosrun sr_example link_joints * rosrun rqt_gui rqt_gui
in the rqt_gui go to plugins->ShadowRobot->joint slider and select EtherCAT hand If you move the joint slider for FFJ3, then MFJ3 will move as well.
Definition in file link_joints.cpp.
void callback | ( | const sr_robot_msgs::JointControllerStateConstPtr & | msg | ) |
The callback function is called each time a message is received on the controller
msg | message of type sr_hand::joints_data |
Definition at line 89 of file link_joints.cpp.
int main | ( | int | argc, |
char ** | argv | ||
) |
The main: initialise a ros node, a subscriber and a publisher
argc | |
argv |
init the subscriber and subscribe to the parent joint controller topic using the callback function callback()
init the publisher on the child joint controller command topic publishing messages of the type std_msgs::Float64.
Definition at line 109 of file link_joints.cpp.
std::string child_name = "mfj3" |
the name of the child joint to link to the parent
Definition at line 70 of file link_joints.cpp.
std::string controller_type = "_mixed_position_velocity_controller" |
the type of controller that will be running
Definition at line 73 of file link_joints.cpp.
std::string parent_name = "ffj3" |
the name of the parent joint
Definition at line 68 of file link_joints.cpp.
Definition at line 80 of file link_joints.cpp.
Definition at line 78 of file link_joints.cpp.