C++ (Cpp) transFromOdomToMapPosition Examples

Example #1

File: go_to_selected_ball.cpp Project: iarczi/elektron_ballcollector

void GoToSelectedBall::publishPose(float dist_from_ball){

	ROS_INFO("enter publishPose, distance = %f", dist_from_ball);

	if(isBallPoseSet== false){
		ROS_INFO("publishPose, wait for ball position, return");
		return;
	}

	float ball_odom_x = getCurrentPose().x;
	float ball_odom_y = getCurrentPose().y;


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


	float dx = ball_odom_x - robot_odom_x;
	float dy = ball_odom_y - robot_odom_y;

	float dist = sqrt(pow(ball_odom_x - robot_odom_x, 2) + pow(ball_odom_y-robot_odom_y, 2) );

	ROS_INFO("dist = %f",dist);

	float goal_odom_x = robot_odom_x + dx * (dist - dist_from_ball) / dist;
	float goal_odom_y = robot_odom_y + dy * (dist - dist_from_ball) / dist;

	float angle = getAngle(robot_odom_x, robot_odom_y, ball_odom_x, ball_odom_y);

	tf::Quaternion q;
	float goal_map_x, goal_map_y;
	transFromOdomToMapPosition(goal_odom_x, goal_odom_y, angle, 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");
	ac.sendGoal(goal);
	firstGoalSent = true;

}

Example #2

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

void Explore::randomRotate(){

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


	float angle = (((rand() % 360) - 180) * PI) / 180.0;

	tf::Quaternion q;
	float goal_map_x, goal_map_y;
	transFromOdomToMapPosition(robot_odom_x, robot_odom_y, angle, 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");
	ac_.sendGoal(goal);

//	ROS_INFO("wait for result");
//	ac_.waitForResult();
	ROS_INFO("leave randomRotate");



}

Example #3

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");
}