wair92's profile - activity

How to do this in C++?

import baxter_interface
import rospy

rospy.init_node("node_name")
limb = baxter_interface.Limb("right")
pose = limb.endpoint_pose()

I tired:

int main(int argc, char **argv)
{
    ros::init(argc, argv, "SKYNET_TESTING");
    ros::NodeHandle nh;
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::Duration(1).sleep();
    double planningTime=20.0;
    double orientationTolerance=0.01;
    double positionTolerance=0.1;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroup group_right_arm("right_arm");
group_right_arm.setPlanningTime(planningTime);
    std::cout << "Planning time: " << group_right_arm.getPlanningTime() << std::endl;
    group_right_arm.setGoalOrientationTolerance(orientationTolerance);
    std::cout << "Orientation Goal Tolerance: " << group_right_arm.getGoalOrientationTolerance() << std::endl;
    group_right_arm.setGoalPositionTolerance(positionTolerance);
    std::cout << "Position Goal Tolearnce " << group_right_arm.getGoalPositionTolerance() << std::endl;
    ros::Duration(3).sleep();
    ROS_INFO_STREAM("Cartesian 1 is now ready...");
    moveit::planning_interface::MoveGroup::Plan arm_right_plan;
    std::vector<geometry_msgs::Pose> arm_right_waypoints;
    moveit_msgs::RobotTrajectory arm_right_trajectory;
    group_right_arm.startStateMonitor();
    group_right_arm.allowReplanning(true);
    vector<double> vec;
    while (ros::ok())
    {
        group_right_arm.getCurrentJointValues();
        geometry_msgs::Pose rpy = group_right_arm.getCurrentPose().pose;
        vec = group_right_arm.getCurrentRPY();
        double x = rpy.position.x ;
        double y = rpy.position.y;
        double z = rpy.position.z;
        ROS_INFO_STREAM(" RPY :"<< x << " "<< y <<" "<< z);
        for(int x = 0; x< vec.size();x++)
        {
            ROS_INFO_STREAM("vector RPY " << vec[x]);
        }
        ros::spinOnce();
    }

}

But with no success, I always got the same values and arm is in different positions. I am on baxter. What should be wrong?