how do get know baxter arm position in c++ code
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?
Asked by wair92 on 2015-07-10 04:32:12 UTC
Comments