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 <sr_robot_msgs/joints_data.h>
#include <sr_robot_msgs/joint.h>
#include <sr_robot_msgs/sendupdate.h>
Go to the source code of this file.
Functions | |
void | callback (const sr_robot_msgs::joints_dataConstPtr &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 | 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/>.
To test this program, just start the hand, rviz visualizer, the control GUI and this example (in 4 different consoles):
roslaunch sr_hand srh_motor.launch roslaunch sr_hand rviz_motor.launch rosrun sr_control_gui __init__.py rosrun sr_hand link_joints
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::joints_dataConstPtr & | msg | ) |
The callback function is called each time a message is received on the topic /srh/shadowhand_data
msg | message of type sr_hand::joints_data |
if it's the parent joint, read the target, and send it to the child.
Definition at line 80 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 topic /srh/shadowhand_data, using the callback function callback()
init the publisher on the topic /srh/sendupdate publishing messages of the type sr_robot_msgs::sendupdate.
Definition at line 135 of file link_joints.cpp.
std::string child_name = "MFJ3" |
the name of the child joint to link to the parent
Definition at line 67 of file link_joints.cpp.
std::string parent_name = "FFJ3" |
the name of the parent joint
Definition at line 65 of file link_joints.cpp.
Definition at line 72 of file link_joints.cpp.
Definition at line 70 of file link_joints.cpp.