def brake(self):
move_velocity = g_msgs.Twist()
move_velocity.linear.x = 0
move_velocity.linear.y = 0
move_velocity.angular.z = 0
self.cmd_angular_pub.publish(move_velocity)
def numpy_to_twist(linear_vel, angular_vel):
return geometry_msgs.Twist(linear=geometry_msgs.Vector3(*linear_vel),
angular=geometry_msgs.Vector3(*angular_vel))
def execute_cb(self, goal):
# helper variables
success = True
#Callback function implementing the pose value received
# publish info to the console for the user
rospy.loginfo('%s: Executing, ' % (self._action_name))
# start executing the action
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: server is Preempted' % self._action_name)
self._as.set_preempted()
success = False
#self._feedback.sequence = ["Preempted",]
#self._feedback.sequence = actionlib.GoalStatus.PREEMPTED
else:
#mitwist=geomsg.Twist(linear=geomsg.Vector3(x=goal.len),angular=geomsg.Vector3(x=goal.ang))
#switched from polar to cartesian
#if goal.ang != 0:
# mitwist=geomsg.Twist(angular=geomsg.Vector3(z=-goal.ang*2*math.pi/360)) #to rad CW
# self.pubtwist.publish(mitwist,)
#if goal.len != 0:
mitwist=geomsg.Twist()
i_msg=0
i_msg_max=100
if not debug_teleport:
while (self.get_distance(goal.x,goal.y) > distance_tolerance) and (i_msg < i_msg_max):
i_msg+=1
#print(goal.x,goal.y, self.pose.x, self.pose.y, distance_tolerance, self.get_distance(goal.x,goal.y))
#Proportional Controller (thanks to https://github.com/clebercoutof/turtlesim_cleaner)
#angular velocity in the z-axis:
mitwist.angular.x = 0
mitwist.angular.y = 0
mitwist.angular.z = 4 * (math.atan2(goal.y - self.pose.y, goal.x - self.pose.x) - self.pose.theta)
#Publishing our vel_msg
self.pubtwist.publish(mitwist)
self.r.sleep()
self.r.sleep()
self.r.sleep()
#linear velocity in the x-axis:
mitwist.linear.x = 1.5 * math.sqrt(pow((goal.x - self.pose.x), 2) + pow((goal.y - self.pose.y), 2))
mitwist.linear.y = 0
mitwist.linear.z = 0
self.pubtwist.publish(mitwist)
self.r.sleep()
if (self.get_distance(goal.x,goal.y) > distance_tolerance):
# looks like there is a problem that makes P controller go into infinite loops, until it's fixed, teleporting
rospy.wait_for_service('/ged/turtle1/teleport_absolute')
try:
teleport = rospy.ServiceProxy('/ged/turtle1/teleport_absolute', TeleportAbsolute)
response = teleport(goal.x,goal.y,0)
print("Teleport response: {}".format(response))
rospy.logwarn("Teleported: {}".format(response))
self.pose_callback(self.pose)
time.sleep(2)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def move_to(self, x=0.0, y=0.0, max_velocity=0.0):
rospy.logerr('Robot Move to x=%s y=%s' % (x, y))
#设置停止回调函数
rospy.on_shutdown(self.brake)
move_velocity = g_msgs.Twist()
# 计算目标移动欧拉距离
goal_distance = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
# 算出方向角,便于接下来的分解
direction_angle = math.atan2(abs(y), abs(x))
# 获取启动前的x，y，yaw
start_x, start_y, start_w = self.robot_state.get_robot_current_x_y_w()
rospy.logerr('start_x=%s start_y=%s' % (start_x, start_y))
# 设置目标插值距离，确定最终插值曲线
self.linear_sp.set_goal(abs(goal_distance), max_velocity)
# 确定目标位置
angular_has_moved = 0.0
is_in_x = False
is_in_y = False
#用于判断是否为单一方向上的运动(针对机器人坐标系)
if x == 0:
is_in_y = True
if y == 0:
is_in_x = True
while not rospy.is_shutdown() and goal_distance != 0:
current_x, current_y, current_w = self.robot_state.get_robot_current_x_y_w(
)
#x 方向已经移动的距离
x_has_moved = current_x - start_x
#y 方向已经移动的距离
y_has_moved = current_y - start_y
#已经走过的距离
dis_has_moved = math.sqrt(
math.pow(x_has_moved, 2) + math.pow(y_has_moved, 2))
#角度的偏移量
angular_has_moved = abs(current_w - start_w)
#计算速度
#如果只走x
if is_in_x == True:
#当x与y方向都到达目标以后退出循环,机器人的速度赋0
#防止机器人到达目标点后无法停止
if abs(dis_has_moved) >= abs(x):
self.brake()
rospy.logerr('We have arrived the goal!!!!')
break
else:
#进行速度赋值
self.x_speed = math.copysign(
self.linear_sp.cal(abs(dis_has_moved)), x)
self.y_speed = 0.0
#只走y方向
elif is_in_y == True:
#当x与y方向都到达目标以后退出循环,机器人的速度赋0
#防止机器人到达目标点后无法停止
if abs(dis_has_moved) >= abs(y):
self.brake()
rospy.logerr('We have arrived the goal!!!!')
break
else:
#进行速度赋值
self.x_speed = 0.0
self.y_speed = math.copysign(
self.linear_sp.cal(abs(dis_has_moved)), y)
#机器人角速度为0
self.w_speed = 0.0
move_velocity.linear.x = self.x_speed
move_velocity.linear.y = self.y_speed
move_velocity.angular.z = self.w_speed
self.cmd_move_pub.publish(move_velocity)
self.rate.sleep()
self.brake()
def numpy_to_twist(linear_vel, angular_vel):
'''TODO: Unit-test
'''
return geometry_msgs.Twist(linear=geometry_msgs.Vector3(*linear_vel), angular=geometry_msgs.Vector3(*angular_vel))
def SendNavigationMessage(self, request, context):
# TODO: This frame_id should be dynamically set from a config file.
nav_header = std_msgs.msg.Header(
frame_id="fosenkaia_NED",
stamp=rospy.Time.from_sec(request.timeStamp)
)
position = geomsgs.Point()
position.x = request.position.x
position.y = request.position.y
position.z = request.position.z
orientation = geomsgs.Quaternion()
orientation.x = request.orientation.x
orientation.y = request.orientation.y
orientation.z = request.orientation.z
orientation.w = request.orientation.w
pose_msg = geomsgs.PoseStamped(
header=nav_header,
pose=geomsgs.Pose(
position=position,
orientation=orientation
)
)
pose_pub.publish(pose_msg)
linear_vel = geomsgs.Vector3()
linear_vel.x = request.linearVelocity.x
linear_vel.y = request.linearVelocity.y
linear_vel.z = request.linearVelocity.z
angular_vel = geomsgs.Vector3()
angular_vel.x = request.angularVelocity.x
angular_vel.y = request.angularVelocity.y
angular_vel.z = request.angularVelocity.z
twist_msg = geomsgs.TwistStamped(
header=nav_header,
twist=geomsgs.Twist(
linear=linear_vel,
angular=angular_vel
)
)
twist_pub.publish(twist_msg)
transform = geomsgs.TransformStamped(
header=nav_header,
child_frame_id="vessel_center",
transform=geomsgs.Transform(
translation=position,
rotation=orientation
)
)
tf_pub.sendTransform(transform)
return navigation_pb2.NavigationResponse(success=True)
def move_to(self, x=0.0, y=0.0, max_velocity=0.0):
rospy.logerr('Move to x=%s y=%s' % (x, y))
#设置停止回调函数
rospy.on_shutdown(self.brake)
move_velocity = g_msgs.Twist()
# 计算目标移动欧拉距离
goal_distance = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
# 算出方向角,便于接下来的分解
direction_angle = math.atan2(abs(y), abs(x))
# 获取启动前的x，y，yaw
start_x, start_y, start_w = self.robot_state.get_robot_current_x_y_w()
rospy.logerr('start_x=%s start_y=%s' % (start_x, start_y))
# 设置目标插值距离，确定最终插值曲线
self.linear_sp.set_goal(abs(goal_distance), max_velocity)
# 确定目标位置
x_goal = start_x + x
y_goal = start_y + y
angular_has_moved = 0.0
y_has_moved = 0.0
x_has_moved = 0.0
x_amend_speed = 0.0
y_amend_speed = 0.0
x_arrive_goal = False
y_arrive_goal = False
move_in_x = False
move_in_y = False
#用于判断是否为单一方向上的运动
if x == 0:
move_in_y = True
x_arrive_goal = True
if y == 0:
move_in_x = True
y_arrive_goal = True
while not rospy.is_shutdown() and goal_distance != 0:
current_x, current_y, current_w = self.robot_state.get_robot_current_x_y_w(
)
#x 方向已经移动的距离
x_has_moved = current_x - start_x
#y 方向已经移动的距离
y_has_moved = current_y - start_y
#角度的偏移量
angular_has_moved = abs(current_w - start_w)
#计算速度
#如果只走x
if move_in_x == True:
#当x与y方向都到达目标以后退出循环,机器人的速度赋0
if x_arrive_goal == True and y_arrive_goal == True:
self.brake()
break
#y方向的速度矫正
if abs(y_has_moved) < self.stop_tolerance:
self.y_speed = 0.0
else:
self.y_speed = math.copysign(self.x_y_amend_speed,
-y_has_moved)
#防止机器人到达目标点后无法停止
if abs(x_has_moved) >= abs(x):
self.brake()
rospy.logerr('We have arrived the goal!!!!')
break
else:
#x方向的速度进行赋值
self.x_speed = math.copysign(
self.linear_sp.cal(abs(x_has_moved)), x)
#角度矫正
if angular_has_moved < math.pi / 180:
self.w_speed = 0.0
else:
self.w_speed = math.copysign(1, current_w)
#只走y方向
elif move_in_y == True:
#当x与y方向都到达目标以后退出循环,机器人的速度赋0
if x_arrive_goal == True and y_arrive_goal == True:
self.brake()
break
#y方向的角度矫正
if abs(x_has_moved) >= self.stop_tolerance:
self.x_speed = math.copysign(self.x_y_amend_speed,
-x_has_moved)
else:
self.x_speed = 0.0
#防止机器人到达目标点后无法停止
if abs(y_has_moved) >= abs(y):
self.brake()
break
else:
#y方向的速度进行赋值
self.y_speed = math.copysign(
self.linear_sp.cal(abs(y_has_moved)), y)
#角度矫正
if angular_has_moved < math.pi / 180:
self.w_speed = 0.0
else:
self.w_speed = math.copysign(1, current_w)
else:
#同时向x与y方向进行运动
#如果x与y方向均到达目标则机器人停止运动
#设置末端矫正
if x_arrive_goal == True and y_arrive_goal == True:
if abs(current_x - x_goal) > 0.02:
linear_move().move_to(x_goal - current_x, 0, 1)
if abs(y - y_has_moved) > 0.02:
linear_move().move_to(0, y_goal - current_y, 1)
break
else:
#设置基于目标角度的误差矫正，需要配合末端矫正才可以使机器人到达准确的位置
current_direction_angle = math.atan2(
abs(current_y), abs(current_x))
dis_has_moved = math.sqrt(x_has_moved**2 + y_has_moved**2)
linear_speed = self.linear_sp.cal(dis_has_moved)
bisu = direction_angle - current_direction_angle
if abs(bisu) > 0.05:
# Y 方向跑的慢
if bisu > 0:
x_amend_speed = 0.0
y_amend_speed = self.amend_speed
# X 方向跑的慢
else:
x_amend_speed = self.amend_speed
y_amend_speed = 0.0
else:
x_amend_speed = 0.0
y_amend_speed = 0.0
#判断x方向是否到目标距离
if abs(x_has_moved) >= abs(x):
self.x_speed = 0.0
x_arrive_goal = True
else:
self.x_speed = math.copysign(
(linear_speed + x_amend_speed) *
math.cos(direction_angle), x)
#判断y方向是否到目标距离
if abs(y_has_moved) >= abs(y):
self.y_speed = 0.0
y_arrive_goal = True
else:
self.y_speed = math.copysign(
(linear_speed + y_amend_speed) *
math.sin(direction_angle), y)
#角度矫正
if angular_has_moved < math.pi / 180:
self.w_speed = 0.0
else:
self.w_speed = math.copysign(1, current_w)
#将计算好的速度赋值并发下去
move_velocity.linear.x = self.x_speed
move_velocity.linear.y = self.y_speed
move_velocity.angular.z = self.w_speed
self.cmd_move_pub.publish(move_velocity)
self.rate.sleep()
self.brake()
def test_set_msg_attribute_value(self):
twi = geo_msgs.Twist()
ez_model.set_msg_attribute_value(twi, '/twist', float, [u'linear', u'x'], None, 0.1)
self.assertEqual(twi.linear.x, 0.1)
self.assertEqual(ez_model.get_msg_attribute_value(twi, '/twist', [u'linear', u'x']), 0.1)