real_state_rviz.cpp
Go to the documentation of this file.
00001 #include <string>
00002 #include <ros/ros.h>
00003 #include <sensor_msgs/JointState.h>
00004 #include <aubo_msgs/JointPos.h>
00005 #include <tf/transform_broadcaster.h>
00006 
00007 aubo_msgs::JointPos cur_pos;
00008 
00009 void chatterCallback1(const aubo_msgs::JointPos::ConstPtr &msg)
00010 {
00011    ROS_INFO("[%f,%f,%f,%f,%f,%f]",msg->joint1,msg->joint2,msg->joint3,msg->joint4,msg->joint5,msg->joint6);
00012    cur_pos.joint1 = msg->joint1;
00013    cur_pos.joint2 = msg->joint2;
00014    cur_pos.joint3 = msg->joint3;
00015    cur_pos.joint4 = msg->joint4;
00016    cur_pos.joint5 = msg->joint5;
00017    cur_pos.joint6 = msg->joint6;
00018 }
00019 
00020 
00021 int main(int argc, char** argv) 
00022 {
00023     ros::init(argc, argv, "real_state_rviz");
00024         ros::NodeHandle n;
00025         ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
00026 
00027         ros::Subscriber sub1 = n.subscribe("current_pos", 1000, chatterCallback1);
00028 
00029         tf::TransformBroadcaster broadcaster;
00030     ros::Rate loop_rate(10);//
00031 
00032         const double degree = M_PI/180;
00033         // robot state
00034     double inc= 0.005, shoulder_inc = 0.003,upper_shoulder_inc = 0.003, fore_upper_inc= 0.003, wrist1_fore_inc= 0.004,wrist2_wrist1_inc= 0.005,wrist3_wrist2_inc= 0.006;
00035     double angle= 0 ,shoulder = 0, upper_shoulder = 0, fore_upper = 0, wrist1_fore = 0, wrist2_wrist1 = 0, wrist3_wrist2 = 0;
00036 
00037     cur_pos.joint1 = 0;
00038         cur_pos.joint2 = 0;
00039         cur_pos.joint3 = 0;
00040         cur_pos.joint4 = 0;
00041         cur_pos.joint5 = 0;
00042         cur_pos.joint6 = 0;
00043         
00044         // message declarations
00045         geometry_msgs::TransformStamped odom_trans;
00046         sensor_msgs::JointState joint_state;
00047     odom_trans.header.frame_id = "base";
00048         odom_trans.child_frame_id = "base_Link";
00049 
00050         while (ros::ok()) 
00051     {
00052                 //update joint_state
00053                 joint_state.header.stamp = ros::Time::now();
00054                 joint_state.name.resize(6);
00055                 joint_state.position.resize(6);
00056                 joint_state.name[0] ="shoulder_joint";
00057                 joint_state.position[0] = cur_pos.joint1;//shoulder;
00058                 joint_state.name[1] ="upperArm_joint";
00059                 joint_state.position[1] = cur_pos.joint2;//upper_shoulder;
00060                 joint_state.name[2] ="foreArm_joint";
00061                 joint_state.position[2] = cur_pos.joint3;//fore_upper;
00062                 joint_state.name[3] ="wrist1_joint";
00063                 joint_state.position[3] = cur_pos.joint4;//wrist1_fore;
00064                 joint_state.name[4] ="wrist2_joint";
00065                 joint_state.position[4] = cur_pos.joint5;//wrist2_wrist1;
00066                 joint_state.name[5] ="wrist3_joint";
00067                 joint_state.position[5] = cur_pos.joint6;//wrist3_wrist2;
00068                 
00069 
00070                 // update transform
00071                 // (moving in a circle with radius)
00072                 odom_trans.header.stamp = ros::Time::now();
00073                 odom_trans.transform.translation.x = cos(angle);
00074                 odom_trans.transform.translation.y = sin(angle);
00075                 odom_trans.transform.translation.z = 0.0;
00076                 odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);
00077                 //send the joint state and transform
00078         joint_pub.publish(joint_state);
00079         //broadcaster.sendTransform(odom_trans);
00080 
00081                 // Create new robot state
00082                 wrist3_wrist2  += wrist3_wrist2_inc;
00083                 if (wrist3_wrist2 <-3.05 || wrist3_wrist2 >3.05) wrist3_wrist2_inc *= -1;
00084 
00085         
00086                 wrist2_wrist1  += wrist2_wrist1_inc;
00087                 if (wrist2_wrist1 <-1.5 || wrist2_wrist1 >1.5) wrist2_wrist1_inc *= -1;
00088 
00089                 wrist1_fore  += wrist1_fore_inc;
00090                 if (wrist1_fore <-1.5 || wrist1_fore >1.5) wrist1_fore_inc *= -1;
00091 
00092 
00093                 fore_upper  += fore_upper_inc;
00094                 if (fore_upper <-1.5 || fore_upper >1.5) fore_upper_inc *= -1;
00095 
00096                 upper_shoulder  += upper_shoulder_inc;
00097                 if (upper_shoulder <-1.5 || upper_shoulder >1.5) upper_shoulder_inc *= -1;
00098 
00099                 shoulder  += shoulder_inc;
00100                 if (shoulder <-3.05 || shoulder >3.05) shoulder_inc *= -1;
00101 
00102 
00103 
00104         //angle += degree/4;
00105 
00106 
00107                 // This will adjust as needed per iteration
00108                 loop_rate.sleep();
00109                 ros::spinOnce();
00110         }
00111     return 0;
00112 }


aubo_control
Author(s): liuxin
autogenerated on Wed Sep 6 2017 03:06:39