C++ (Cpp) getRobotAngleInOdom Examples

Example #1

File: go_to_selected_ball.cpp Project: iarczi/elektron_ballcollector

float GoToSelectedBall::getAngleDiff(){

	float robot_odom_x, robot_odom_y;
	getRobotPositionInOdom(robot_odom_x, robot_odom_y);

	float goalAngle = getAngle(robot_odom_x, robot_odom_y, getCurrentPose().x, getCurrentPose().y);
	float robotAngle = getRobotAngleInOdom();

	return fabs(goalAngle-robotAngle);

}

Example #2

File: explore.cpp Project: RCPRG-ros-pkg/elektron_ballcollector

void Explore::robotFullRotate(){

	ROS_INFO("enter robotFullRotate ");
	float robot_odom_x, robot_odom_y;
	getRobotPositionInOdom(robot_odom_x, robot_odom_y);


	float angle_in_odom = getRobotAngleInOdom();		//	kat od -PI do + PI
	float anglePlusPI = addPiToAngle(angle_in_odom);

	tf::Quaternion q;
	float goal_map_x, goal_map_y;
	transFromOdomToMapPosition(robot_odom_x, robot_odom_y, anglePlusPI, goal_map_x, goal_map_y, q);

	geometry_msgs::Quaternion qMsg;
	tf::quaternionTFToMsg(q, qMsg);


	move_base_msgs::MoveBaseGoal goal;

	goal.target_pose.pose.position.x = goal_map_x;
	goal.target_pose.pose.position.y = goal_map_y;
	goal.target_pose.pose.position.z = 0;


	goal.target_pose.pose.orientation = qMsg;

	goal.target_pose.header.stamp = ros::Time::now();
	goal.target_pose.header.frame_id ="/map";

//	ROS_INFO("Sending goal 1");
	ac_.sendGoal(goal);

	/*

	ROS_INFO("wait for result");
	ac_.waitForResult();


	getRobotPositionInOdom(robot_odom_x, robot_odom_y);


	angle_in_odom = getRobotAngleInOdom();		//	kat od -PI do + PI
	anglePlusPI = addPiToAngle(angle_in_odom);

	transFromOdomToMapPosition(robot_odom_x, robot_odom_y, anglePlusPI, goal_map_x, goal_map_y, q);

	tf::quaternionTFToMsg(q, qMsg);


	goal.target_pose.pose.position.x = goal_map_x;
	goal.target_pose.pose.position.y = goal_map_y;
	goal.target_pose.pose.position.z = 0;


	goal.target_pose.pose.orientation = qMsg;

	goal.target_pose.header.stamp = ros::Time::now();
	goal.target_pose.header.frame_id ="/map";

	ROS_INFO("Sending goal 2");
	ac_.sendGoal(goal);

	ROS_INFO("wait for result");
	ac_.waitForResult();


*/
	ROS_INFO("leave robotFullRotate");
}