test_jaco_arm_controller.cpp
Go to the documentation of this file.
00001 //============================================================================
00002 // Name        : Jaco.cpp
00003 // Author      : 
00004 // Version     :
00005 // Copyright   : Your copyright notice
00006 // Description : Hello World in C++, Ansi-style
00007 //============================================================================
00008 
00017 //License File
00018 #include <geometry_msgs/PoseStamped.h>
00019 #include <geometry_msgs/TwistStamped.h>
00020 #include <tf/tf.h>
00021 #include <ros/ros.h>
00022 #include <jaco_driver/JacoPositionConfig.h>
00023 #include <dynamic_reconfigure/server.h>
00024 using namespace std;
00025 
00026 
00027 
00028 ros::Publisher *pub;
00029 
00030 float x_pose=0;
00031 float    y_pose=0;
00032 float    z_pose=0;
00033 float    rx_pose=0;
00034 float    ry_pose=0;
00035 float    rz_pose=0;
00036 
00037 
00038 void callback(jaco_driver::JacoPositionConfig &config, uint32_t level) {
00039 
00040          x_pose=config.X_Pose;
00041          y_pose=config.Y_Pose;
00042          z_pose=config.Z_Pose;
00043          rx_pose=config.RX_Pose;
00044          ry_pose=config.RY_Pose;
00045          rz_pose=config.RZ_Pose;
00046 
00047         //      ROS_INFO("Reconfigure");
00048 
00049 
00050 }
00051 
00052 
00053 
00054 void TimerCallback(const ros::TimerEvent&)
00055 {
00056         geometry_msgs::PoseStamped test_msg;
00057 
00058                         test_msg.pose.position.x = x_pose;
00059                         test_msg.pose.position.y = y_pose;
00060                         test_msg.pose.position.z = z_pose;
00061 
00062                         tf::Quaternion q;
00063 
00064                         q.setRPY(rx_pose,ry_pose,rz_pose);
00065 
00066                         tf::quaternionTFToMsg(q,test_msg.pose.orientation);
00067                         test_msg.header.frame_id = "/arm_base";
00068                         test_msg.header.stamp = ros::Time().now();
00069                         pub->publish(test_msg);
00070 
00071 }
00072 
00073 int main(int argc, char **argv) {
00074 
00075         /* Set up ROS */
00076         ros::init(argc, argv, "test_jaco_arm_controller");
00077         ros::NodeHandle nh;
00078         ros::NodeHandle param_nh("~");
00079 
00080         std::string ArmPose("object_pose"); 
00081 
00082         ros::Publisher pub2 = nh.advertise<geometry_msgs::PoseStamped>(ArmPose,
00083                         2);
00084 
00085         pub = &pub2;
00086         dynamic_reconfigure::Server<jaco_driver::JacoPositionConfig>  dr_server;
00087  dynamic_reconfigure::Server<jaco_driver::JacoPositionConfig>::CallbackType     dr_call = boost::bind(&callback, _1, _2);
00088         dr_server.setCallback(dr_call);
00089           ros::Timer    timer = nh.createTimer(ros::Duration(0.1),&TimerCallback);
00090 
00091                 ros::spin();
00092 
00093 }
00094 


jaco_driver
Author(s): Ilia Baranov (Clearpath) , Jeff Schmidt (Clearpath) , Alex Bencz (Clearpath) , Matt DeDonato (WPI), Braden Stenning
autogenerated on Mon Mar 2 2015 16:21:03