#!/usr/bin/env python
import rospy
import re #regular expressions
import serial
from geometry_msgs.msg import Pose2D
pub = rospy.Publisher('imu', Pose2D, queue_size=1)
rospy.init_node('imu_node')
rate = rospy.Rate(50) #Hz
pose = Pose2D()
portname = rospy.get_param("/imu_node/serial_port", "/dev/ttyACM0")
serial_port = serial.Serial(portname, 115200)
serial_port.flushInput()
while not rospy.is_shutdown():
try:
serial_port.write('I')
buf = serial_port.readline()
m = re.findall(r"\d+\.\d*", buf)
rospy.loginfo(m)
pose.theta = float(m[0])
pub.publish(pose)
rate.sleep()
except:
pass
def arr_to_pose2D(self, arr, ang, pose = Pose2D()):
pose.x = arr[0] + self.odom_pos.x
pose.y = arr[1] + self.odom_pos.y
pose.theta = ang
return pose
def main():
rospy.init_node('ai', anonymous=False)
# Create robot objects that store that current robot's state
_create_robots()
global _ball
_ball = Ball()
# Subscribe to Robot States
rospy.Subscriber('my_state', RobotState, lambda msg: _handle_robot_state(msg, 'me'))
rospy.Subscriber('ally_state', RobotState, lambda msg: _handle_robot_state(msg, 'ally'))
rospy.Subscriber('opponent1_state', RobotState, lambda msg: _handle_robot_state(msg, 'opp1'))
rospy.Subscriber('opponent2_state', RobotState, lambda msg: _handle_robot_state(msg, 'opp2'))
rospy.Subscriber('ball_state', BallState, _handle_ball_state)
# This message will tell us if we are to be playing or not right now
rospy.Subscriber('/game_state', GameState, _handle_game_state)
pub = rospy.Publisher('desired_position', Pose2D, queue_size=10)
rate = rospy.Rate(100) #100 Hz
while not rospy.is_shutdown():
# Was there a goal to tell Strategy about?
if _game_state['us_goal']:
goal = Strategy.G.US
_game_state['us_goal'] = False
elif _game_state['them_goal']:
goal = Strategy.G.THEM
_game_state['them_goal'] = False
else:
goal = Strategy.G.NO_ONE
# We didn't name this well ha. So 'not' it
one_v_one = not _game_state['two_v_two']
(x_c, y_c, theta_c) = Strategy.choose_strategy(_me, _ally, _opp1, _opp2, _ball, \
was_goal=goal, one_v_one=one_v_one)
# Get a message ready to send
msg = Pose2D()
if _game_state['play']:
# Run AI as normal
msg.x = x_c
msg.y = y_c
msg.theta = theta_c
else:
# Send robot to home
if _me.ally1:
msg.x = Constants.ally1_start_pos[0]
msg.y = Constants.ally1_start_pos[1]
msg.theta = Constants.ally1_start_pos[2]
elif _me.ally2:
msg.x = Constants.ally2_start_pos[0]
msg.y = Constants.ally2_start_pos[1]
msg.theta = Constants.ally2_start_pos[2]
# If it's not the first time, the robot will 'hold its ground'
# even while paused. (See guidedog_node.py)
if not _game_state['first_time']:
pub.publish(msg)
rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def run_navigator(self):
""" computes a path from current state to goal state using A* and sends it to the path controller """
# makes sure we have a location
try:
(translation, rotation) = self.trans_listener.lookupTransform(
'/map', '/base_footprint', rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
euler = tf.transformations.euler_from_quaternion(rotation)
self.theta = euler[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
self.current_plan = []
return
# makes sure we have a map
if not self.occupancy:
self.current_plan = []
return
# if close to the goal, use the pose_controller instead
if self.close_to_end_location():
#rospy.loginfo("Navigator: Close to nav goal using pose controller")
pose_g_msg = Pose2D()
pose_g_msg.x = self.x_g
pose_g_msg.y = self.y_g
pose_g_msg.theta = self.theta_g
self.nav_pose_pub.publish(pose_g_msg)
self.current_plan = []
self.V_prev = 0
return
# if there is no plan, we are far from the start of the plan,
# or the occupancy grid has been updated, update the current plan
if len(self.current_plan) == 0 or not (
self.close_to_start_location()) or self.occupancy_updated:
# use A* to compute new plan
state_min = self.snap_to_grid(
(-self.plan_horizon, -self.plan_horizon))
state_max = self.snap_to_grid(
(self.plan_horizon, self.plan_horizon))
x_init = self.snap_to_grid((self.x, self.y))
x_goal = self.snap_to_grid((self.x_g, self.y_g))
problem = AStar(state_min, state_max, x_init, x_goal,
self.occupancy, self.plan_resolution)
#rospy.loginfo("Navigator: Computing navigation plan")
#rospy.loginfo("Self at %f %f %f", self.x, self.y, self.theta)
#rospy.loginfo("Goal is %f %f %f", self.x_g, self.y_g, self.theta_g)
if problem.solve():
#rospy.loginfo("There is a path")
if len(problem.path) > 3:
#rospy.loginfo("Computing splines")
# cubic spline interpolation requires 4 points
self.current_plan = problem.path
self.current_plan_start_time = rospy.get_rostime()
self.current_plan_start_loc = [self.x, self.y]
self.occupancy_updated = False
# publish plan for visualization
path_msg = Path()
path_msg.header.frame_id = 'map'
for state in self.current_plan:
pose_st = PoseStamped()
pose_st.pose.position.x = state[0]
pose_st.pose.position.y = state[1]
pose_st.pose.orientation.w = 1
pose_st.header.frame_id = 'map'
path_msg.poses.append(pose_st)
self.nav_path_pub.publish(path_msg)
path_t = [0]
path_x = [self.current_plan[0][0]]
path_y = [self.current_plan[0][1]]
for i in range(len(self.current_plan) - 1):
dx = self.current_plan[i +
1][0] - self.current_plan[i][0]
dy = self.current_plan[i +
1][1] - self.current_plan[i][1]
path_t.append(path_t[i] +
np.sqrt(dx**2 + dy**2) / V_DES)
path_x.append(self.current_plan[i + 1][0])
path_y.append(self.current_plan[i + 1][1])
# interpolate the path with cubic spline
self.path_x_spline = scipy.interpolate.splrep(path_t,
path_x,
k=3,
s=SMOOTH)
self.path_y_spline = scipy.interpolate.splrep(path_t,
path_y,
k=3,
s=SMOOTH)
self.path_tf = path_t[-1]
# to inspect the interpolation and smoothing
# t_test = np.linspace(path_t[0],path_t[-1],1000)
# plt.plot(path_t,path_x,'ro')
# plt.plot(t_test,scipy.interpolate.splev(t_test,self.path_x_spline,der=0))
# plt.plot(path_t,path_y,'bo')
# plt.plot(t_test,scipy.interpolate.splev(t_test,self.path_y_spline,der=0))
# plt.show()
else:
rospy.logwarn("Navigator: Path too short, not updating")
else:
rospy.logwarn("Navigator: Could not find path")
pose_g_msg = Pose2D()
pose_g_msg.x = self.x
pose_g_msg.y = self.y
self.x_g = self.x
self.y_g = self.y
self.theta_g = self.theta
pose_g_msg.theta = self.theta
self.nav_pose_pub.publish(pose_g_msg)
self.current_plan = []
self.V_prev = 0
return
# if we have a path, execute it (we need at least 3 points for this controller)
if len(self.current_plan) > 3:
#rospy.loginfo("Executing path")
# if currently not moving, first line up with the plan
if self.V_prev == 0:
#rospy.loginfo("Currently stopped. Trying to go to path")
t_path_init = (rospy.get_rostime() -
self.current_plan_start_time).to_sec()
theta_init = np.arctan2(
self.current_plan[1][1] - self.current_plan[0][1],
self.current_plan[1][0] - self.current_plan[0][0])
theta_err = theta_init - self.theta
if abs(theta_err) > THETA_START_THRESH:
cmd_msg = Twist()
cmd_msg.linear.x = 0
cmd_msg.angular.z = THETA_START_P * theta_err
self.nav_vel_pub.publish(cmd_msg)
return
# compute the "current" time along the path execution
t = (rospy.get_rostime() - self.current_plan_start_time).to_sec()
t = max(0.0, t)
t = min(t, self.path_tf)
x_d = scipy.interpolate.splev(t, self.path_x_spline, der=0)
y_d = scipy.interpolate.splev(t, self.path_y_spline, der=0)
xd_d = scipy.interpolate.splev(t, self.path_x_spline, der=1)
yd_d = scipy.interpolate.splev(t, self.path_y_spline, der=1)
xdd_d = scipy.interpolate.splev(t, self.path_x_spline, der=2)
ydd_d = scipy.interpolate.splev(t, self.path_y_spline, der=2)
# publish current desired x and y for visualization only
pathsp_msg = PoseStamped()
pathsp_msg.header.frame_id = 'map'
pathsp_msg.pose.position.x = x_d
pathsp_msg.pose.position.y = y_d
theta_d = np.arctan2(yd_d, xd_d)
quat_d = tf.transformations.quaternion_from_euler(0, 0, theta_d)
pathsp_msg.pose.orientation.x = quat_d[0]
pathsp_msg.pose.orientation.y = quat_d[1]
pathsp_msg.pose.orientation.z = quat_d[2]
pathsp_msg.pose.orientation.w = quat_d[3]
self.nav_pathsp_pub.publish(pathsp_msg)
if self.V_prev <= 0.0001:
self.V_prev = linalg.norm([xd_d, yd_d])
dt = (rospy.get_rostime() - self.V_prev_t).to_sec()
xd = self.V_prev * np.cos(self.theta)
yd = self.V_prev * np.sin(self.theta)
u = np.array([
xdd_d + KPX * (x_d - self.x) + KDX * (xd_d - xd),
ydd_d + KPY * (y_d - self.y) + KDY * (yd_d - yd)
])
J = np.array(
[[np.cos(self.theta), -self.V_prev * np.sin(self.theta)],
[np.sin(self.theta), self.V_prev * np.cos(self.theta)]])
a, om = linalg.solve(J, u)
V = self.V_prev + a * dt
# apply saturation limits
cmd_x_dot = np.sign(V) * min(V_MAX, np.abs(V))
cmd_theta_dot = np.sign(om) * min(W_MAX, np.abs(om))
elif len(self.current_plan) > 0:
# using the pose controller for paths too short
# just send the next point
pose_g_msg = Pose2D()
pose_g_msg.x = self.current_plan[0][0]
pose_g_msg.y = self.current_plan[0][1]
if len(self.current_plan) > 1:
pose_g_msg.theta = np.arctan2(
self.current_plan[1][1] - self.current_plan[0][1],
self.current_plan[1][0] - self.current_plan[0][0])
else:
pose_g_msg.theta = self.theta_g
self.nav_pose_pub.publish(pose_g_msg)
return
else:
# just stop
cmd_x_dot = 0
cmd_theta_dot = 0
# saving the last velocity for the controller
self.V_prev = cmd_x_dot
self.V_prev_t = rospy.get_rostime()
cmd_msg = Twist()
cmd_msg.linear.x = cmd_x_dot
cmd_msg.angular.z = cmd_theta_dot
self.nav_vel_pub.publish(cmd_msg)
def go_to_goal(self):
pose_g_msg = Pose2D()
pose_g_msg.x = self.cur_goal[0]
pose_g_msg.y = self.cur_goal[1]
def init(args):
# Initialize all publishers, subscribers, state variables
global g_namespace, g_pose, g_ir_sensors, g_us_sensor, g_world_starting_pose, g_world_obstacles, g_world_surface, g_motors, g_ping_requested, g_servo_angle
global g_pub_state, g_pub_odom, g_sub_motors, g_sub_ping, g_sub_odom, g_sub_servo, g_render_requested, g_sub_render
global g_tk_window, g_tk_label, g_render_image_base
g_namespace = args.namespace
rospy.init_node("sparki_simulator_%s" % g_namespace)
g_ir_sensors = [0] * 5
g_us_sensor = -1
g_motors = [0., 0.]
g_servo_angle = 0
g_ping_requested = False
g_world_starting_pose = Pose2D()
g_world_starting_pose.x, g_world_starting_pose.y, g_world_starting_pose.theta = args.startingpose[
0], args.startingpose[1], args.startingpose[2]
g_pose = copy.copy(g_world_starting_pose)
g_world_obstacles = load_img_to_bool_matrix(args.obstacles)
g_world_surface = load_img_to_bool_matrix(args.worldmap)
if (g_world_obstacles.size != g_world_surface.size):
g_world_obstacles = load_img_to_bool_matrix(None)
g_world_surface = load_img_to_bool_matrix(None)
rospy.logerr(
"Obstacle map and surface map have different dimensions. Resetting to blank!"
)
MAP_SIZE_X = g_world_obstacles.shape[1]
MAP_SIZE_Y = g_world_obstacles.shape[0]
g_pub_odom = rospy.Publisher("/%s/odometry" % g_namespace,
Pose2D,
queue_size=10)
g_pub_state = rospy.Publisher('/%s/state' % g_namespace,
String,
queue_size=10)
g_sub_motors = rospy.Subscriber('/%s/motor_command' % g_namespace,
Float32MultiArray, recv_motor_command)
g_sub_ping = rospy.Subscriber('/%s/ping_command' % g_namespace, Empty,
recv_ping)
g_sub_odom = rospy.Subscriber('/%s/set_odometry' % g_namespace, Pose2D,
set_odometry)
g_sub_servo = rospy.Subscriber('/%s/set_servo' % g_namespace, Int16,
set_servo)
g_sub_render = rospy.Subscriber('/%s/render_sim' % g_namespace, Empty,
recv_render)
g_tk_window = tk.Tk()
img = ImageTk.PhotoImage('RGB', (MAP_SIZE_X, MAP_SIZE_Y))
g_tk_label = tk.Label(g_tk_window, image=img)
g_tk_label.pack(fill="both", expand="yes")
g_render_requested = True
g_render_image_base = Image.new('RGB', (MAP_SIZE_X, MAP_SIZE_Y),
color='white')
for y_coord in range(0, MAP_SIZE_Y):
for x_coord in range(0, MAP_SIZE_X):
# Add line-following diagram as shades of black-to-red
if g_world_surface[y_coord, x_coord] > 0:
g_render_image_base.putpixel(
(x_coord, y_coord),
(255 - int(g_world_surface[y_coord, x_coord] / 5), 0, 0))
# Add objects as shades of black-to-green
if g_world_obstacles[y_coord, x_coord] > 0:
g_render_image_base.putpixel(
(x_coord, y_coord),
(0, 255 - int(g_world_surface[y_coord, x_coord] / 5), 0))
FT = F.T
pp = np.dot(F, np.dot(P, FT)) + V
y = gps - np.dot(H, xp)
S = np.dot(H, np.dot(pp, HT)) + W
SI = np.linalg.inv(S)
kal = np.dot(pp, np.dot(HT, SI))
xf = xp + np.dot(kal, y)
P = pp - np.dot(kal, np.dot(H, pp))
gpsfil = Pose2D()
gpsfil.x = xf[0]
gpsfil.y = xf[1]
gpsfil.theta = xf[2]
pub.publish(gpsfil)
estPose = PoseStamped()
estPose.header.frame_id = "map"
estPose.pose.position.x = xf[0]
estPose.pose.position.y = xf[1]
quaternion = tf.transformations.quaternion_from_euler(0, 0, xf[2])
estPose.pose.orientation.x = quaternion[0]
estPose.pose.orientation.y = quaternion[1]
estPose.pose.orientation.z = quaternion[2]
estPose.pose.orientation.w = quaternion[3]
def update_odometry(self):
# Get wheel encoder ticks
ticks = self.robot.get_wheel_ticks()
# Have not seen a wheel encoder message yet so no need to do anything
if ticks["r"] == None or ticks["l"] == None:
return
# Robot may not start with encoder count at zero
if self.prev_wheel_ticks == None:
self.prev_wheel_ticks = {"r": ticks["r"], "l": ticks["l"]}
rospy.loginfo(rospy.get_caller_id() + " initial r ticks: " +
str(ticks["r"]))
rospy.loginfo(rospy.get_caller_id() + " initial l ticks: " +
str(ticks["l"]))
# If ticks are the same since last time, then no need to update either
if ticks["r"] == self.prev_wheel_ticks["r"] and \
ticks["l"] == self.prev_wheel_ticks["l"]:
return
# Get current pose from robot
prev_pose = self.robot.pose2D
# Compute odometry - kinematics in meters
R = self.robot.wheel_radius
L = self.robot.wheelbase
ticks_per_rev = self.robot.encoder_ticks_per_rev
meters_per_tick = (2.0 * math.pi * R) / ticks_per_rev
# How far did each wheel move
meters_right = \
meters_per_tick * (ticks["r"] - self.prev_wheel_ticks["r"])
meters_left = \
meters_per_tick * (ticks["l"] - self.prev_wheel_ticks["l"])
meters_center = (meters_right + meters_left) * 0.5
# Compute new pose
x_dt = meters_center * math.cos(prev_pose.theta)
y_dt = meters_center * math.sin(prev_pose.theta)
theta_dt = (meters_right - meters_left) / L
new_pose = Pose2D(0.0, 0.0, 0.0)
new_pose.x = prev_pose.x + x_dt
new_pose.y = prev_pose.y + y_dt
new_pose.theta = prev_pose.theta + theta_dt
if True:
rospy.loginfo(rospy.get_caller_id() + " prev r ticks: " +
str(self.prev_wheel_ticks["r"]))
rospy.loginfo(rospy.get_caller_id() + " prev l ticks: " +
str(self.prev_wheel_ticks["l"]))
rospy.loginfo(rospy.get_caller_id() + " r ticks: " +
str(ticks["r"]))
rospy.loginfo(rospy.get_caller_id() + " l ticks: " +
str(ticks["l"]))
rospy.loginfo(rospy.get_caller_id() + " x: " + str(new_pose.x))
rospy.loginfo(rospy.get_caller_id() + " y: " + str(new_pose.y))
rospy.loginfo(rospy.get_caller_id() + " theta: " +
str(new_pose.theta))
# Update robot with new pose
self.robot.pose2D = new_pose
# Update the tick count
self.prev_wheel_ticks["r"] = ticks["r"]
self.prev_wheel_ticks["l"] = ticks["l"]
# Broadcast pose as ROS tf
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "world"
t.child_frame_id = "rosbots_robot"
t.transform.translation.x = new_pose.x
t.transform.translation.y = new_pose.y
t.transform.translation.z = 0.0
q = tf.transformations.quaternion_from_euler(0, 0, new_pose.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
br.sendTransform(t)
def callback(self, data):
# 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print e
## 裁剪原始图片
h, w, ch = cv_image.shape
img_trim = cv_image[50:320, 210:520]
## 滤波&转换色彩空间
img_blur1 = cv2.bilateralFilter(img_trim, 9, 75, 75)
img_blur2 = cv2.blur(img_blur1, (5, 5))
img_hsv = cv2.cvtColor(img_blur2, cv2.COLOR_BGR2HSV)
## 设定object的HSV色彩空间阈值范围
lower_obj = np.array([81, 50, 0])
higher_obj = np.array([154, 139, 255])
## 根据HSV色彩空间范围区分object和后景
img_mask = cv2.inRange(img_hsv, lower_obj, higher_obj)
## 寻找轮廓
contours, hierarchy = cv2.findContours(img_mask, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
cnt = contours[0]
bg_contours = np.zeros(img_trim.shape, dtype=np.uint8)
contours_result = cv2.drawContours(bg_contours, [cnt], 0, (0, 255, 0),
2)
## 轮廓逼近拟合
epsilon = 0.01 * cv2.arcLength(cnt, True)
approx = cv2.approxPolyDP(cnt, epsilon, True)
## 分析几何形状
corners = len(approx)
## 绘制拟合轮廓
bg_approx = np.zeros(img_trim.shape, dtype=np.uint8)
approx_result = cv2.drawContours(bg_approx, [approx], -1, (255, 0, 0),
3)
## 求解中心位置
# 设定初始值
cx = 0
cy = 0
mm = cv2.moments(approx)
if mm['m00'] != 0:
cx = int(mm['m10'] / mm['m00'])
cy = int(mm['m01'] / mm['m00'])
result = cv2.circle(approx_result, (cx, cy), 3, (0, 0, 255), -1)
else:
pass
# print "中心位置坐标", cx, cy
img_result = approx_result
# print cv_image.shape
# 显示Opencv格式的图像
# cv2.imshow("Image window", cv_image)
# cv2.waitKey(3)
object_pose = Pose2D()
object_pose.x = cx
object_pose.y = cy
object_pose.theta = 0
# 再将opencv格式额数据转换成ros image格式的数据发布
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(img_result, "bgr8"))
self.coordinate_pub.publish(object_pose)
except CvBridgeError as e:
print e
def _indirect_control_update(self):
# Controller経由でロボットを操縦する
MOVE_GAIN = 0.04 # meters
MOVE_GAIN_ANGLE = 0.04 * math.pi # radians
control_enable = True
# メッセージを取得してない場合は抜ける
if self._joy_msg is None:
return
color_or_id_changed = False
current_joy_pose = self._joy_target.path[-1]
# シャットダウン
if self._joy_msg.buttons[self._BUTTON_SHUTDOWN_1] and\
self._joy_msg.buttons[self._BUTTON_SHUTDOWN_2]:
rospy.signal_shutdown('finish')
return
# チームカラーの変更
if self._joy_msg.buttons[self._BUTTON_COLOR_ENABLE]:
# カラー変更直後は操縦を停止する
self._indirect_control_enable = False
if math.fabs(self._joy_msg.axes[self._AXIS_COLOR_CHANGE]) > 0:
self._is_yellow = not self._is_yellow
print 'is_yellow: ' + str(self._is_yellow)
color_or_id_changed = True
# キーが離れるまでループ
while self._joy_msg.axes[self._AXIS_COLOR_CHANGE] != 0:
pass
# IDの変更
if self._joy_msg.buttons[self._BUTTON_ID_ENABLE]:
# ID変更直後は操縦を停止する
self._indirect_control_enable = False
if math.fabs(self._joy_msg.axes[self._AXIS_ID_CHANGE]) > 0:
# 十字キーの入力に合わせて、IDを増減させる
self._robot_id += int(self._joy_msg.axes[self._AXIS_ID_CHANGE])
if self._robot_id > self._MAX_ID:
self._robot_id = self._MAX_ID
if self._robot_id < 0:
self._robot_id = 0
print 'robot_id:' + str(self._robot_id)
color_or_id_changed = True
# キーが離れるまでループ
while self._joy_msg.axes[self._AXIS_ID_CHANGE] != 0:
pass
# poseの更新
if self._joy_msg.buttons[self._BUTTON_MOVE_ENABLE]:
# 操縦を許可する
self._indirect_control_enable = True
if math.fabs(self._joy_msg.axes[self._AXIS_VEL_SWAY]):
# joystickの仕様により符号を反転している
gain = MOVE_GAIN * self._joy_msg.axes[self._AXIS_VEL_SWAY]
current_joy_pose.x += math.copysign(
gain, -self._joy_msg.axes[self._AXIS_VEL_SWAY])
if math.fabs(self._joy_msg.axes[self._AXIS_VEL_SURGE]):
gain = MOVE_GAIN * self._joy_msg.axes[self._AXIS_VEL_SURGE]
current_joy_pose.y += math.copysign(
gain, self._joy_msg.axes[self._AXIS_VEL_SURGE])
if math.fabs(self._joy_msg.axes[self._AXIS_VEL_ANGULAR]):
gain = MOVE_GAIN_ANGLE * self._joy_msg.axes[
self._AXIS_VEL_ANGULAR]
current_joy_pose.theta += math.copysign(
gain, self._joy_msg.axes[self._AXIS_VEL_ANGULAR])
current_joy_pose.theta = angle_normalize(
current_joy_pose.theta)
# パスの末尾にセット
self._joy_target.path[-1] = current_joy_pose
# Pathの操作
if self._joy_msg.buttons[self._BUTTON_PATH_ENABLE]:
# 操縦を許可する
self._indirect_control_enable = True
# Poseの追加
if self._joy_msg.buttons[self._BUTTON_ADD_POSE]:
self._joy_target.path.append(copy.deepcopy(current_joy_pose))
print 'add pose' + str(len(self._joy_target.path))
# キーが離れるまでループ
while self._joy_msg.buttons[self._BUTTON_ADD_POSE] != 0:
pass
# Pathの初期化
if self._joy_msg.buttons[self._BUTTON_DELETE_PATH]:
self._joy_target.path = []
self._joy_target.path.append(Pose2D())
print 'delete path'
# キーが離れるまでループ
while self._joy_msg.buttons[self._BUTTON_DELETE_PATH] != 0:
pass
# ControlTargetの送信
if self._joy_msg.buttons[self._BUTTON_SEND_TARGET]:
self._joy_target.robot_id = self._robot_id
self._joy_target.control_enable = True
color = 'blue'
if self._is_yellow:
color = 'yellow'
# 末尾に16進数の文字列をつける
topic_id = hex(self._robot_id)[2:]
topic_name = 'consai2_game/control_target_' + color + '_' + topic_id
pub_control_target = rospy.Publisher(topic_name,
ControlTarget,
queue_size=1)
pub_control_target.publish(self._joy_target)
# self._pub_control_target.publish(self._joy_target)
print 'send target'
# Color, ID変更ボタンを押すことで操縦を停止できる
if self._indirect_control_enable is False:
stop_target = ControlTarget()
stop_target.robot_id = self._robot_id
stop_target.control_enable = False
color = 'blue'
if self._is_yellow:
color = 'yellow'
# 末尾に16進数の文字列をつける
topic_id = hex(self._robot_id)[2:]
topic_name = 'consai2_game/control_target_' + color + '_' + topic_id
pub_control_target = rospy.Publisher(topic_name,
ControlTarget,
queue_size=1)
pub_control_target.publish()
self._pub_joy_target.publish(self._joy_target)
def __init__(self):
self._MAX_ID = rospy.get_param('consai2_description/max_id')
# 直接操縦かcontrol経由の操縦かを決める
self._DIRECT = rospy.get_param('~direct')
# /consai2_examples/launch/joystick_example.launch でキー割り当てを変更する
self._BUTTON_SHUTDOWN_1 = rospy.get_param('~button_shutdown_1')
self._BUTTON_SHUTDOWN_2 = rospy.get_param('~button_shutdown_2')
self._BUTTON_MOVE_ENABLE = rospy.get_param('~button_move_enable')
self._AXIS_VEL_SURGE = rospy.get_param('~axis_vel_surge')
self._AXIS_VEL_SWAY = rospy.get_param('~axis_vel_sway')
self._AXIS_VEL_ANGULAR = rospy.get_param('~axis_vel_angular')
self._BUTTON_KICK_ENABLE = rospy.get_param('~button_kick_enable')
self._BUTTON_KICK_STRAIGHT = rospy.get_param('~button_kick_straight')
self._BUTTON_KICK_CHIP = rospy.get_param('~button_kick_chip')
self._AXIS_KICK_POWER = rospy.get_param('~axis_kick_power')
self._BUTTON_DRIBBLE_ENABLE = rospy.get_param('~button_dribble_enable')
self._AXIS_DRIBBLE_POWER = rospy.get_param('~axis_dribble_power')
self._BUTTON_ID_ENABLE = rospy.get_param('~button_id_enable')
self._AXIS_ID_CHANGE = rospy.get_param('~axis_id_change')
self._BUTTON_COLOR_ENABLE = rospy.get_param('~button_color_enable')
self._AXIS_COLOR_CHANGE = rospy.get_param('~axis_color_change')
self._BUTTON_ALL_ID_1 = rospy.get_param('~button_all_id_1')
self._BUTTON_ALL_ID_2 = rospy.get_param('~button_all_id_2')
self._BUTTON_ALL_ID_3 = rospy.get_param('~button_all_id_3')
self._BUTTON_ALL_ID_4 = rospy.get_param('~button_all_id_4')
# indirect control用の定数
self._BUTTON_PATH_ENABLE = rospy.get_param('~button_path_enable')
self._BUTTON_ADD_POSE = rospy.get_param('~button_add_pose')
self._BUTTON_DELETE_PATH = rospy.get_param('~button_delete_path')
self._BUTTON_SEND_TARGET = rospy.get_param('~button_send_target')
self._MAX_VEL_SURGE = 1.0
self._MAX_VEL_SWAY = 1.0
self._MAX_VEL_ANGULAR = math.pi
self._MAX_KICK_POWER = 1.0 # DO NOT EDIT
self._KICK_POWER_CONTROL = 0.1
self._MAX_DRIBBLE_POWER = 1.0 # DO NOT EDIT
self._DRIBBLE_POWER_CONTROL = 0.1
self._indirect_control_enable = True
# direct control用のパラメータ
self._kick_power = 0.5
self._dribble_power = 0.5
self._robot_id = 0
self._is_yellow = False
self._all_member = False
self._pub_commands = rospy.Publisher('consai2_control/robot_commands',
RobotCommands,
queue_size=1)
self._joy_msg = None
self._sub_joy = rospy.Subscriber('joy',
Joy,
self._callback_joy,
queue_size=1)
self._pub_joy_target = rospy.Publisher('consai2_examples/joy_target',
ControlTarget,
queue_size=1)
self._joy_target = ControlTarget()
self._joy_target.path.append(Pose2D()) # スタート位置をセット
def _decode_referee(self, msg):
decoded_msg = DecodedReferee()
decoded_msg.referee_id = self._decode_referee_id(msg.command)
decoded_msg.referee_text = REFEREE_TEXT.get(decoded_msg.referee_id, 'INVALID_COMMAND')
decoded_msg.placement_position = msg.designated_position
# Decode restrictions
if decoded_msg.referee_id == self._DECODE_ID["HALT"] \
or decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["GOAL"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["GOAL"]:
# Reference : Rule 2019, 5.1.2 Halt
decoded_msg.can_move_robot = False
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = False
decoded_msg.can_enter_their_side = False
decoded_msg.can_enter_center_circle = False
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT
elif decoded_msg.referee_id == self._DECODE_ID["STOP"]:
# Reference : Rule 2019, 5.1.1 Stop
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._SPEED_LIMIT_OF_ROBOT
decoded_msg.can_kick_ball = False
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._KEEP_OUT_RADIUS_FROM_BALL
decoded_msg.keep_out_distance_from_their_defense_area = \
self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA
elif decoded_msg.referee_id == self._DECODE_ID["FORCE_START"]:
# Reference : Rule 2019, 5.3.5 Force Start
# Reference : Rule 2019, 8.1.6 Ball Speed
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = True
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT
elif decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["KICKOFF_PREPARATION"]:
# Reference : Rule 2019, 5.3.2 Kick-Off
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = False
decoded_msg.can_enter_their_side = False
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = 0 # No limit but robot do not touch the ball
decoded_msg.keep_out_distance_from_their_defense_area = \
self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA
elif decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["KICKOFF_START"]:
# Reference : Rule 2019, 5.3.1 Normal Start
# Reference : Rule 2019, 5.3.2 Kick-Off
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = True
decoded_msg.can_enter_their_side = False
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = \
self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA
elif decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["PENALTY_PREPARATION"]:
# Reference : Rule 2019, 5.3.6 Penalty Kick
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = False
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = 0 # No limit but robot do not touch the ball
decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT
elif decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["PENALTY_START"]:
# Reference : Rule 2019, 5.3.1 Normal Start
# Reference : Rule 2019, 5.3.6 Penalty Kick
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = True
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT
elif decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["DIRECT_FREE"] \
or decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["INDIRECT_FREE"]:
# Reference : Rule 2019, 5.3.3 Direct Free Kick
# Reference : Rule 2019, 5.3.6 Indirect Free Kick
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = True
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = \
self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA
elif decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["BALL_PLACEMENT"]:
# Reference : Rule 2019, 5.2 Ball Placement
# Reference : Rule 2019, 8.2.8 Robot Stop Speed
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = True
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = \
self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA
elif decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["KICKOFF_PREPARATION"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["KICKOFF_START"]:
# Reference : Rule 2019, 5.3.2 Kick-Off
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = False
decoded_msg.can_enter_their_side = False
decoded_msg.can_enter_center_circle = False
decoded_msg.keep_out_radius_from_ball = self._KEEP_OUT_RADIUS_FROM_BALL
decoded_msg.keep_out_distance_from_their_defense_area = \
self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA
elif decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["PENALTY_PREPARATION"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["PENALTY_START"]:
# Reference : Rule 2019, 5.3.6 Penalty Kick
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = False
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._KEEP_OUT_RADIUS_FROM_BALL
decoded_msg.keep_out_distance_from_their_defense_area = \
self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA
elif decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["DIRECT_FREE"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["INDIRECT_FREE"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["BALL_PLACEMENT"]:
# Reference : Rule 2019, 5.3.3 Direct Free Kick
# Reference : Rule 2019, 5.3.6 Indirect Free Kick
# Reference : Rule 2019, 8.2.3 Ball Placement Interference
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = False
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._KEEP_OUT_RADIUS_FROM_BALL
decoded_msg.keep_out_distance_from_their_defense_area = \
self._KEEP_OUT_DISTANCE_FROM_THEIR_DEFENSE_AREA
elif decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["TIMEOUT"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["TIMEOUT"]:
# Reference : Rule 2019, 4.4.2 Timeouts
# No limitations
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = True
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT
else:
decoded_msg.can_move_robot = False
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = False
decoded_msg.can_enter_their_side = False
decoded_msg.can_enter_center_circle = False
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT
# Consider inplay
# Reference : Rule 2019, 8.1.3 Double Touch
# Reference : Rule 2019, A.1 Ball In And Out Of Play
if decoded_msg.referee_id == self._DECODE_ID["STOP"] \
or decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["KICKOFF_PREPARATION"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["KICKOFF_PREPARATION"] \
or decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["PENALTY_PREPARATION"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["PENALTY_PREPARATION"] \
or decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["BALL_PLACEMENT"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["BALL_PLACEMENT"]:
self._stationary_ball_pose = self._ball_pose
self._game_is_inplay = False
elif decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["KICKOFF_START"] \
or decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["PENALTY_START"] \
or decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["DIRECT_FREE"] \
or decoded_msg.referee_id == self._DECODE_ID["OUR"] + self._DECODE_ID["INDIRECT_FREE"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["KICKOFF_START"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["PENALTY_START"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["DIRECT_FREE"] \
or decoded_msg.referee_id == self._DECODE_ID["THEIR"] + self._DECODE_ID["INDIRECT_FREE"]:
# ボールが静止位置から動いたかを判断する
if self._game_is_inplay is False:
diff_pose = Pose2D()
diff_pose.x = self._ball_pose.x - self._stationary_ball_pose.x
diff_pose.y = self._ball_pose.y - self._stationary_ball_pose.y
move_distance = math.hypot(diff_pose.x, diff_pose.y)
if move_distance > self._INPLAY_DISTANCE:
self._game_is_inplay = True
# インプレイのときは行動制限を解除する
if self._game_is_inplay is True:
decoded_msg.can_move_robot = True
decoded_msg.speed_limit_of_robot = self._NO_LIMIT
decoded_msg.can_kick_ball = True
decoded_msg.can_enter_their_side = True
decoded_msg.can_enter_center_circle = True
decoded_msg.keep_out_radius_from_ball = self._NO_LIMIT
decoded_msg.keep_out_distance_from_their_defense_area = self._NO_LIMIT
decoded_msg.referee_text += "(INPLAY)"
return decoded_msg
# Gimbal Action Defines:
from math import pi
from geometry_msgs.msg import Pose2D
LEFT = -90
RIGHT = 90
MIDDLE = 0
PARTROL_RATE = 1
BUFFERZONES = [[0.9, 2, 85, pi], [2.075, 1, 85, pi], [4.32, 4.05, pi],
[7.4, 1.95, 0], [6.175, 3.05, 0], [4.1, 0.65, pi]]
defences = [[2.0, 3.8, -pi / 4], [5, 3.8, -3 * pi / 4], [2.0, 1.2, pi / 4],
[6, 1.2, 3 * pi / 4], [3, 2.4, 0], [4.2, 3, 0], [5, 2.4, pi / 3],
[4.2, 2, 0]]
DEFENCEPOINTS = [Pose2D(x=i[0], y=i[1], theta=i[2]) for i in defences]
searchs = [[5.54, 3.9, 0], [6.73, 0.82, 3.14], [2.49, 4.00, 3.14],
[1.26, 1.02, 0]]
BARRIERS = [[[0.205, 3.28], [1.205, 3.28], [0.205, 3.48], [1.205, 3.48]],
[[1.705, 0.205], [1.905, 0.205], [1.705, 1.205], [1.905, 1.205]],
[[1.705, 2.345], [2.505, 2.345], [1.705, 2.545], [2.505, 2.545]],
[[3.975, 1.140], [4.975, 1.140], [3.975, 1.340], [4.975, 1.340]],
[[3.975, 3.550], [4.975, 3.550], [3.975, 3.750], [4.975, 3.750]],
[[4.045, 2.445], [4.245, 2.645], [4.445, 2.445], [4.245, 2.245]],
[[6.045, 2.345], [6.845, 2.345], [6.045, 2.545], [6.845, 2.545]],
[[6.585, 3.885], [6.785, 3.885], [6.585, 4.685], [6.785, 4.685]],
[[7.285, 1.205], [8.285, 1.205], [7.285, 1.405], [8.285, 1.405]]]
SEARCHZONES = [Pose2D(x=i[0], y=i[1], theta=i[2]) for i in searchs]
import rospy
import json
import copy
import time
from geometry_msgs.msg import Pose2D
from std_msgs.msg import Float32MultiArray, Empty, String, Int16
import math
from math import cos, sin
from numpy import rot90
import os
# GLOBALS
pose2d_sparki_odometry = Pose2D(
0, 0, 0
) #Pose2D message object, contains x,y,theta members in meters and radians
servo_angle = None
ping_distance = None
ir_sensor_readings = [0 for i in range(5)]
height = 24
width = 36
max_dist = math.sqrt(width**2 + height**2)
map_coord = [0 for n in range(width * height)]
map_cell_size = 0.05
robot_map_index = 0
render_count = 0
# TODO: Use these variables to hold your publishers and subscribers
publisher_motor = None
publisher_odom = None
publisher_ping = None
publisher_servo = None
def callback(self, msg):
N = len(msg.readings)
V = msg.vel_trans
W = msg.vel_ang
self.lx = []
self.ly = []
self.lr = []
self.lb = []
for i in range(N):
self.lx.append(msg.readings[i].landmark.x)
self.ly.append(msg.readings[i].landmark.y)
self.lb.append(msg.readings[i].bearing)
self.lr.append(msg.readings[i].range)
#######################
#Extended Kalman Filter
angle=true.xyt[2]
F=numpy.zeros((3,3))
F[0,0] =1
F[0,2] =-self.timestep*V*numpy.sin(angle)
F[1,1] =1
F[1,2] =self.timestep*V*numpy.cos(angle)
F[2,2] =1
xpp=self.timestep*numpy.array([V*numpy.cos(angle),V*numpy.sin(angle),W])
xp=numpy.array(true.xyt)+ xpp
Fp=numpy.dot(F,true.P)
Ppp=numpy.dot(Fp, numpy.transpose(F))
Pp=Ppp+self.covariance*numpy.identity(3)
if N > 0:
y=numpy.empty(2*N)
H=numpy.empty((2*N,3))
Hx=numpy.empty(2*N)
for i in range(N):
k=2*i
y[k]=self.lr[i]
y[k+1]=self.lb[i]
H[k][0]=(xp[0]-self.lx[i])/numpy.sqrt((xp[0]-self.lx[i])**2+(xp[1]-self.ly[i])**2)
H[k][1]=(xp[1]-self.ly[i])/numpy.sqrt((xp[0]-self.lx[i])**2+(xp[1]-self.ly[i])**2)
H[k][2]=0
H[k+1][0]=-(xp[1]-self.ly[i])/((xp[0]-self.lx[i])**2+(xp[1]-self.ly[i])**2)
H[k+1][1]=(xp[0]-self.lx[i])/((xp[0]-self.lx[i])**2+(xp[1]-self.ly[i])**2)
H[k+1][2]=-1
Hx[k]=numpy.sqrt((xp[0]-self.lx[i])**2+(xp[1]-self.ly[i])**2)
Hx[k+1]=math.atan2((self.ly[i]-xp[1]),(self.lx[i]-xp[0]))- xp[2]
mu=y-Hx
Sp=numpy.dot(H, Pp)
Spp=numpy.dot(Sp, numpy.transpose(H))
S=Spp+self.covariance*numpy.identity(2*N)
Rp=numpy.dot(Pp, numpy.transpose(H))
R=numpy.dot(Rp, numpy.linalg.inv(S))
xp=xp+numpy.dot(R, mu)
Pp=Pp-numpy.dot(numpy.dot(R,H), Pp)
true.xyt=list(xp)
true.P=Pp
For_publish=Pose2D()
For_publish.x=xp[0]
For_publish.y=xp[1]
For_publish.theta=xp[2]
self.pub.publish(For_publish)
################
#Particle Filter
Wp=numpy.random.normal(0,0.1,(120,3))
Vp=numpy.random.normal(0,0.1,(120,3))
Vp[:,0]=Vp[:,0]+V
Vp[:,1]=Vp[:,1]+V
Vp[:,2]=Vp[:,2]+W
for i in range(120):
Vp[i,0]=Vp[i,0]*numpy.cos(true2.xyt[i,2])
Vp[i,1]=Vp[i,1]*numpy.sin(true2.xyt[i,2])
Xp=self.timestep*Vp+true2.xyt+Wp
if N > 0:
weight=numpy.ones(120)
for i in range(N):
for j in range(120):
sqra=Xp[j][0]-self.lx[i]
sqrb=Xp[j][1]-self.ly[i]
sqr=numpy.sqrt((sqra)**2+(sqrb)**2)
weight[j]=weight[j]*numpy.abs(sqr-self.lr[i])
atana=self.ly[i]-Xp[j][1]
atanb=self.lx[i]-Xp[j][0]
atan=math.atan2(atana,atanb)
value = atan-Xp[j][2]-self.lb[i]
while value > numpy.pi:
value=value-2*numpy.pi
while value < -numpy.pi:
value=value+2*numpy.pi
weight[j]=weight[j]*numpy.abs(value)
weight=1/weight
lottaryS=sum(weight)
for i in range(120):
lottaryV=random.uniform(0,lottaryS)
choice=0
for j in range(120):
choice=choice+weight[j]
if choice > lottaryV:
true2.xyt[i]=Xp[j]
break
else:
true2.xyt = Xp
For_publish=Pose2D()
For_publish.x=sum(true2.xyt[:,0])/120
For_publish.y=sum(true2.xyt[:,1])/120
For_publish.theta=sum(true2.xyt[:,2])/120
self.pub2.publish(For_publish)
quad_list = [quad.x,quad.y,quad.z,quad.w]
(roll,pitch,yaw) = euler_from_quaternion(quad_list)
robot.x = pose.x
robot.y = pose.y
#robot.z = pose.z
robot.theta = yaw
return robot
if __name__ == '__main__':
rospy.init_node('source_pub')
rate = rospy.Rate(100)
robot1_pose_pub = rospy.Publisher('robot1/s_pose',Pose2D,queue_size=10)
robot2_pose_pub = rospy.Publisher('robot2/s_pose',Pose2D,queue_size=10)
robot1 = Pose2D()
robot2 = Pose2D()
Arr_s = []
while not rospy.is_shutdown():
robot1 = source('Robot1')
robot2 = source('Robot2')
robot1_pose_pub.publish(robot1)
robot2_pose_pub.publish(robot2)
Arr_s.append((robot1.x,robot1.y))
Arr_s.append((robot1.x,robot1.y))
# for i in range(n):
# Arr_s.append((robo))
def __init__(self):
print("Programa se iniciou...")
self.pose = Pose2D()
self.simulator()
def draw(self):
if (self.meas_np is None):
print("no measures")
return
if ((self.grads is None and self.values is None) and not minimal):
print("no grads or vals when not minimal")
return
if (not self.sync_map and not minimal):
print("no map updated")
return
if (not self.sync_meas):
print("no meas updated")
return
self.sync_map = False
self.sync_meas = False
self.fig.clear()
if (not minimal):
xy_range = np.arange(self.ranges[0], self.ranges[1],
self.ranges[2])
x, y = np.meshgrid(xy_range, xy_range)
if (not grad):
plt.pcolormesh(x,
y,
self.values,
edgecolors="none",
antialiased=True)
else:
plt.quiver(y,
x,
self.grads[:, 0],
self.grads[:, 1],
minshaft=0.1)
setpt_dir = Pose2D()
pos_zero = Pose2D()
setpt_zero = Pose2D()
setpt_dir.x = np.cos(self.setpt.theta)
setpt_dir.y = np.sin(self.setpt.theta)
setpt_zero.x = 1
setpt_zero.y = 0
self.draw_turtle(self.setpt, setpt_dir)
self.draw_turtle(pos_zero, setpt_zero, arrow_color="g")
if (hist):
self.draw_history()
plt.scatter(self.target.x, self.target.y, 10.1, "r")
plt.scatter(self.meas_np[:, 0], self.meas_np[:, 1], 2.5)
plt.grid()
plt.gca().set_xlim(-2.5, 2.5)
plt.gca().set_aspect('equal', adjustable='box')
plt.draw()
if (save_replay):
plt.savefig("replays/{0:04d}.png".format(self.frame_no),
bbox_inches='tight',
pad_inches=0)
self.frame_no += 1
def test_rosmsg_to_numpy_custom(self):
'''Test a rosmsg conversion for a custom msg'''
pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14)
numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta'])
np.testing.assert_allclose(np.array([1.0, 2.0, 3.14]), numpy_array)
line_sub = rospy.Subscriber('line', Segment, update_line)
line = Segment()
# --------------------------------------------------------------------------------
# Publisher position
# --------------------------------------------------------------------------------
pose_pub = rospy.Publisher('car/pose', Pose2D, queue_size=1)
# --------------------------------------------------------------------------------
# Simulation
# --------------------------------------------------------------------------------
rate = rospy.Rate(10)
cmd = Twist()
# plt.ion()
while not rospy.is_shutdown():
plt.clf()
# Simulation
car.simulate(cmd.linear.x, cmd.angular.z)
# Affichage
img = car.draw()
plt.plot(img[0], img[1])
plt.plot([line.entry.x, line.exit.x], [line.entry.y, line.exit.y])
plt.axis([car.x - 50, car.x + 50, car.y - 50, car.y + 50])
print 'Car position: {}, {}, {}'.format(car.x, car.y, car.theta)
# Publication pose
pose = Pose2D(x=car.x, y=car.y, theta=car.theta)
pose_pub.publish(pose)
plt.pause(rate.sleep_dur.to_sec())
rate.sleep()
def interpose(ball_info, robot_info, control_target):
# ボールの位置
ball_pose = ball_info.pose
# ボールの速度
ball_vel = ball_info.velocity
# ゴールの位置
OUR_GOAL_POSE = Field.goal_pose('our', 'center')
xr = OUR_GOAL_POSE.x + 0.3
goalie_threshold_y = 1.5
# 敵のロボットの情報
robot_info_their = robot_info['their']
dist = []
for their_info in robot_info_their:
if their_info.detected:
dist.append(tool.distance_2_poses(their_info.pose, ball_pose))
else:
dist.append(100)
# 一番近い敵ロボットのID
min_dist_id = dist.index(min(dist))
# 一番近い敵ロボットの座標
robot_pose = robot_info_their[min_dist_id].pose
# ボールの速度
if ball_vel is None:
v = 0
dx = 0
dy = 0
da = 0
else:
# ボールの速度
vx = ball_vel.x
vy = ball_vel.y
v = math.hypot(ball_vel.x, ball_vel.y)
# ボールの進む角度
angle_ball = math.atan2(vy, vx)
# ボールの変化量を計算（正確ではなく方向を考慮した単位量）
dvx = math.cos(angle_ball)
dvy = math.sin(angle_ball)
# ボールの次の予測位置を取得
ball_pose_next = Pose2D(ball_pose.x + dvx, ball_pose.y + dvy, 0)
if dist[min_dist_id] < 0.15 and robot_pose.x < 0:
rospy.logdebug('their')
angle_their = robot_pose.theta
if angle_their == 0:
a = 1e-10
else:
a = math.tan(angle_their)
b = robot_pose.y - a * robot_pose.x
elif 0.02 < v and dvx < 0:
rospy.logdebug('ball move')
a, b = line_pram(ball_pose, ball_pose_next)
else:
rospy.logdebug('ball stop')
a, b = line_pram(ball_pose, OUR_GOAL_POSE)
# 位置を決める
yr = a*xr + b
# 位置
if yr > goalie_threshold_y:
yr = goalie_threshold_y
elif yr < -goalie_threshold_y:
yr = -goalie_threshold_y
# elif ball_pose.x < xr:
# ボールが後ろに出たらy座標は0にする
# yr = 0
new_goal_pose = Pose2D(xr, yr, 0)
# ---------------------------------------------------------
remake_path = False
# pathが設定されてなければpathを新規作成
if control_target.path is None or len(control_target.path) == 0:
remake_path = True
# 現在のpathゴール姿勢と、新しいpathゴール姿勢を比較し、path再生成の必要を判断する
if remake_path is False:
current_goal_pose = control_target.path[-1]
if not tool.is_close(current_goal_pose, new_goal_pose, Pose2D(0.1, 0.1, math.radians(10))):
remake_path = True
# remake_path is Trueならpathを再生成する
# pathを再生成すると衝突回避用に作られた経路もリセットされる
if remake_path:
control_target.path = []
control_target.path.append(new_goal_pose)
# print(goalie_pose.x, goalie_pose.y, ball_pose.x, ball_pose.y)
return control_target
def create(self):
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
pick1_group = 'robot1'
robot1_loc = Pose2D(x=3.8, y=2.1, theta=-90.0)
gripper1 = "vacuum_gripper1_suction_cup"
robot2_loc = Pose2D(x=-4.3, y=-0.9, theta=0.0)
pick2_group = 'robot2'
gripper2 = "vacuum_gripper2_suction_cup"
names2 = [
'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
'robot2_elbow_joint', 'robot2_wrist_1_joint',
'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
]
# x:31 y:236, x:594 y:345
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.robot1_loc = robot1_loc
_state_machine.userdata.pose_turtlebot = []
_state_machine.userdata.pick1_configuration = []
_state_machine.userdata.place1_configuration = []
_state_machine.userdata.Conveyor_speed = 100
_state_machine.userdata.robot2_loc = robot2_loc
_state_machine.userdata.pick2_configuration = []
_state_machine.userdata.place2_configuration = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:32 y:56
OperatableStateMachine.add(
'Move R1 Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'start conveyor belt',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1122 y:138
OperatableStateMachine.add('Compute pick',
ComputeGraspState(group=pick1_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Activate Gripper 1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:381 y:59
OperatableStateMachine.add('Start feeder',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Wait for part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:718 y:59
OperatableStateMachine.add('Stop feeder',
ControlFeederState(activation=False),
transitions={
'succeeded': 'stop conveyor belt',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1144 y:627
OperatableStateMachine.add('Compute place Turtlebot',
ComputeGraspState(group=pick1_group,
offset=0.6,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Move R1 to place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:1139 y:545
OperatableStateMachine.add(
'LocateTurtlebot',
LocateFactoryDeviceState(model_name='mobile_base',
output_frame_id='world'),
transitions={
'succeeded': 'Compute place Turtlebot',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose_turtlebot'})
# x:1125 y:370
OperatableStateMachine.add(
'Move R1 back Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Navigate to robot1',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1147 y:801
OperatableStateMachine.add(
'Move R1 back to Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Navigate to robot2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1113 y:59
OperatableStateMachine.add(
'Detect Part Camera',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Compute pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:556 y:60
OperatableStateMachine.add('Wait for part',
SubscriberState(
topic='/break_beam_sensor_change',
blocking=True,
clear=True),
transitions={
'received': 'Stop feeder',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:1121 y:304
OperatableStateMachine.add(
'Move R1 to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move R1 back Home',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:1156 y:688
OperatableStateMachine.add(
'Move R1 to place',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Deactivate Gripper 1',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:211 y:55
OperatableStateMachine.add('start conveyor belt',
SetConveyorPowerState(stop=False),
transitions={
'succeeded': 'Start feeder',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'Conveyor_speed'})
# x:898 y:58
OperatableStateMachine.add('stop conveyor belt',
SetConveyorPowerState(stop=True),
transitions={
'succeeded': 'Detect Part Camera',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'Conveyor_speed'})
# x:1132 y:451
OperatableStateMachine.add('Navigate to robot1',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'LocateTurtlebot',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot1_loc'})
# x:1144 y:745
OperatableStateMachine.add('Deactivate Gripper 1',
VacuumGripperControlState(
enable=False,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 back to Home',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:949 y:816
OperatableStateMachine.add('Navigate to robot2',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'Detect Part Camera_2',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot2_loc'})
# x:1104 y:212
OperatableStateMachine.add('Activate Gripper 1',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:744 y:831
OperatableStateMachine.add(
'Detect Part Camera_2',
DetectPartCameraState(ref_frame='robot2_base',
camera_topic='/hrwros/logical_camera_2',
camera_frame='logical_camera_2_frame'),
transitions={
'continue': 'Compute pick_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:582 y:813
OperatableStateMachine.add('Compute pick_2',
ComputeGraspState(group=pick2_group,
offset=0.0,
joint_names=names2,
tool_link=gripper2,
rotation=3.1415),
transitions={
'continue': 'Activate Gripper 2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick2_configuration',
'joint_names': 'joint_names'
})
# x:401 y:813
OperatableStateMachine.add('Activate Gripper 2',
VacuumGripperControlState(
enable=True,
service_name='/gripper2/control'),
transitions={
'continue': 'Move R2 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:242 y:789
OperatableStateMachine.add(
'Move R2 to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick2_group,
offset=0.0,
tool_link=gripper2,
action_topic='/move_group'),
transitions={
'reached': 'Move R2 to back home',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick2_configuration',
'joint_names': 'joint_names'
})
# x:30 y:566
OperatableStateMachine.add(
'Move R2 to R2Place',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R2Place',
move_group=pick2_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'Deactivate Gripper 2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:0 y:484
OperatableStateMachine.add('Deactivate Gripper 2',
VacuumGripperControlState(
enable=False,
service_name='/gripper2/control'),
transitions={
'continue': 'finished',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:29 y:779
OperatableStateMachine.add(
'Move R2 to back home',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'Move R2 to R2Place',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine
def _process_img(msg):
# print 'vision msg: camera'
global isFirst, field, fgbg
# print 'hello everyone'
# return
array = np.fromstring(msg.data, np.uint8)
uncompressed_img = cv2.imdecode(array, 1)
image = uncompressed_img
# image = _ros2cv(uncompressed_img)
# image = _ros2cv(msg)
# _show_raw(image)
# shiny pink ball - 255, 255, 255, 221, 0, 0
# dull pink ball - 255, 160, 192, 204, 1, 0
# blue - 255, 173, 121, 214, 73, 65
# purple - 252, 63, 174, 214, 0, 127
# red - 250, 170, 56, 201, 94, 0
# green - 250, 124, 81, 170, 29, 57
# yellow - 255, 36, 97, 218, 0, 0
fieldColor = [189, 108, 215, 123, 25, 31]
ourRobot1Color = [250, 170, 56, 201, 94, 0]
ourRobot2Color = [250, 124, 81, 170, 29, 57]
ballColor = [255, 160, 192, 204, 1,
0] #ballColor=[255, 145, 238, 177, 6, 153]
opponent1Color = [229, 216, 241, 204, 195, 222]
opponent2Color = [229, 216, 241, 204, 195, 222]
if isFirst:
field = _field(image, fieldColor, isFirst)
# fgbg = cv2.bgsegm.createBackgroundSubtractorMOG()
# fgbg = cv2.createBackgroundSubtractorMOG2()
global us1_pub, us2_pub, ball_pub, field_dim_pub, field_pos_pub
dim_msg = Pose2D()
dim_msg.x = field[2]
dim_msg.y = field[3]
dim_msg.theta = 0
field_dim_pub.publish(dim_msg)
pos_msg = Pose2D()
pos_msg.x = field[0]
pos_msg.y = field[1]
pos_msg.theta = 0
field_pos_pub.publish(pos_msg)
if all(field):
ourRobot1 = _findRobot(image, ourRobot1Color, "ourRobot1", isFirst,
(3, 3), field)
if (ourRobot1[0] is not None and ourRobot1[1]
is not None): #angle can be zero so cant use all() func
us1_msg = convert_coordinates(ourRobot1[0], ourRobot1[1],
ourRobot1[2], field)
# print 'x_old = {}, y_old = {}'.format(ourRobot1[0], ourRobot1[1])
us1_pub.publish(_orient(us1_msg))
ourRobot2 = _findRobot(image, ourRobot2Color, "ourRobot2", isFirst,
(3, 3), field)
if (ourRobot2[0] is not None and ourRobot2[1]
is not None): #angle can be zero so cant use all() func
us2_msg = convert_coordinates(ourRobot2[0], ourRobot2[1],
ourRobot2[2], field)
us2_pub.publish(_orient(us2_msg))
ball = _ball(image, ballColor, isFirst, field)
if (ball[0] is not None and ball[1] is not None):
ball_msg = convert_coordinates(ball[0], ball[1], ball[2], field)
ball_pub.publish(_orient(ball_msg))
if not _live:
cv2.waitKey(3) #the windows dont stay open if this isnt here...
isFirst = False
def to_Pose2D(self, theta = 0):
return Pose2D(self.x_, self.y_, theta)
from mobrob_util.msg import ME439WheelSpeeds, ME439PathSpecs #, ME439PathSegComplete
from geometry_msgs.msg import Pose2D
from std_msgs.msg import Bool
import me439_mobile_robot_class_v02 as m439rbt # REMEMBER to call the right file (version, or use the _HWK if needed)
# global variable to hold the path specs currently being tracked
path_segment_spec = ME439PathSpecs()
path_segment_spec.x0 = 0.
path_segment_spec.y0 = 0.
path_segment_spec.theta0 = 0.
path_segment_spec.Radius = np.inf
path_segment_spec.Length = np.inf
# global variables used in path following:
initialize_psi_world= True
estimated_pose_previous = Pose2D()
estimated_x_local_previous = 0.
estimated_theta_local_previous= 0.
path_segment_curvature_previous = 0.
estimated_psi_world_previous = 0.
estimated_segment_completion_fraction_previous = 0.
# =============================================================================
# Set up a closed-loop path controller
# Subscribe to "robot_pose_estimated" (Pose2D)
# and Publish "wheel_speeds_desired" (ME439WheelSpeeds)
# =============================================================================
# Get parameters from rosparam
wheel_width = rospy.get_param('/wheel_width_model') # All you have when planning is a model - you never quite know the truth!
def explore_linear(node_list, parent, direction, goal, resolution, bonus,
h_angle, v_angle, size, check_srv):
if direction == 0:
x_move = 0.0
y_move = 1.0
explored_dir = 2
parallel_orientation = 1
perpendicular_orientation = 0
perpendicular_check_goal = goal.x
perpendicular_check_parent = parent.x
parallel_check_goal = goal.y
parallel_check_parent = parent.y
parallel_theta = v_angle
perpendicular_theta = h_angle
mid_explored_plus = [0, 0, 1, -1]
mid_explored_minus = [0, -1, 1, 0]
mid_explored_both = [0, 0, 1, 0]
mid_explored_stop = [0, -1, 1, -1]
end_explored = [-1, 0, 1, 0]
elif direction == 1:
x_move = 1.0
y_move = 0.0
explored_dir = 3
parallel_orientation = 0
perpendicular_orientation = 1
perpendicular_check_goal = goal.y
perpendicular_check_parent = parent.y
parallel_check_goal = goal.x
parallel_check_parent = parent.x
parallel_theta = h_angle
perpendicular_theta = v_angle
mid_explored_plus = [0, 0, -1, 1]
mid_explored_minus = [-1, 0, 0, 1]
mid_explored_both = [0, 0, 0, 1]
mid_explored_stop = [-1, 0, -1, 1]
end_explored = [0, -1, 0, 1]
elif direction == 2:
x_move = 0.0
y_move = -1.0
explored_dir = 0
parallel_orientation = 1
perpendicular_orientation = 0
perpendicular_check_goal = goal.x
perpendicular_check_parent = parent.x
parallel_check_goal = goal.y
parallel_check_parent = parent.y
parallel_theta = v_angle
perpendicular_theta = h_angle
mid_explored_plus = [1, 0, 0, -1]
mid_explored_minus = [1, -1, 0, 0]
mid_explored_both = [1, 0, 0, 0]
mid_explored_stop = [1, -1, 0, -1]
end_explored = [-1, 0, -1, 0]
elif direction == 3:
x_move = -1.0
y_move = 0.0
explored_dir = 1
parallel_orientation = 0
perpendicular_orientation = 1
perpendicular_check_goal = goal.y
perpendicular_check_parent = parent.y
parallel_check_goal = goal.x
parallel_check_parent = parent.x
parallel_theta = h_angle
perpendicular_theta = v_angle
mid_explored_plus = [0, 1, -1, 0]
mid_explored_minus = [-1, 1, 0, 0]
mid_explored_both = [0, 1, 0, 0]
mid_explored_stop = [-1, 1, -1, 0]
end_explored = [0, -1, 0, -1]
#Pose2D objects for test points
check_fwd_a = Pose2D()
check_fwd_b = Pose2D()
check_plus_a = Pose2D()
check_plus_b = Pose2D()
check_minus_a = Pose2D()
check_minus_b = Pose2D()
#if goal lies along direction of exploration, check if path to goal is clear
if abs(perpendicular_check_goal - perpendicular_check_parent
) < size * 0.06 and goal.orientation == parent.orientation:
check_fwd_a.x, check_fwd_a.y, check_fwd_a.theta = parent.x, parent.y, parent.theta
check_fwd_b.x, check_fwd_b.y, check_fwd_b.theta = goal.x, goal.y, goal.theta
resp = check_srv(check_fwd_a, check_fwd_b)
#rospy.loginfo("Goal check:" + repr(resp.valid))
if resp.valid:
goal.parent = parent
return goal
#if exploration direction is towards the goal, check point orthogonal to goal
check_ortho = False
if direction == 1 or direction == 0:
if goal.orientation != parent.orientation and parallel_check_goal > parallel_check_parent:
check_ortho = True
elif direction == 3 or direction == 2:
if goal.orientation != parent.orientation and parallel_check_goal < parallel_check_parent:
check_ortho = True
offset = size * 0.025
if direction == 0 or direction == 2:
check_plus_a.x = parent.x + offset
check_plus_b.x = check_plus_a.x + 0.006
check_minus_a.x = parent.x - offset
check_minus_b.x = check_minus_a.x - 0.006
check_plus_a.y = parent.y
check_plus_b.y = parent.y
check_minus_a.y = parent.y
check_minus_b.y = parent.y
elif direction == 1 or direction == 3:
check_plus_a.x = parent.x
check_plus_b.x = parent.x
check_minus_a.x = parent.x
check_minus_b.x = parent.x
check_plus_a.y = parent.y + offset
check_plus_b.y = check_plus_a.y + 0.006
check_minus_a.y = parent.y - offset
check_minus_b.y = check_minus_a.y - 0.006
check_plus_a.theta = perpendicular_theta
check_plus_b.theta = perpendicular_theta
check_minus_a.theta = perpendicular_theta
check_minus_b.theta = perpendicular_theta
last_node = parent
check_fwd_a.theta = parent.theta
check_fwd_b.theta = parent.theta
check_fwd_b.x = parent.x + x_move * resolution
check_fwd_b.y = parent.y + y_move * resolution
check_fwd_a.x = check_fwd_b.x - x_move * 0.006
check_fwd_a.y = check_fwd_b.y - y_move * 0.006
check_plus_a.x += x_move * resolution
check_plus_b.x += x_move * resolution
check_minus_a.x += x_move * resolution
check_minus_b.x += x_move * resolution
check_plus_a.y += y_move * resolution
check_plus_b.y += y_move * resolution
check_minus_a.y += y_move * resolution
check_minus_b.y += y_move * resolution
last_check = False
ortho_intersect = False
while (True):
#check forward area in direction of exploration
resp = check_srv(check_fwd_a, check_fwd_b)
if resp.valid:
check_fwd_a.x += x_move * resolution
check_fwd_a.y += y_move * resolution
check_fwd_b.x += x_move * resolution
check_fwd_b.y += y_move * resolution
check_plus_a.x += x_move * resolution
check_plus_b.x += x_move * resolution
check_minus_a.x += x_move * resolution
check_minus_b.x += x_move * resolution
check_plus_a.y += y_move * resolution
check_plus_b.y += y_move * resolution
check_minus_a.y += y_move * resolution
check_minus_b.y += y_move * resolution
else:
#if corridor has ended, determine if intersection at end
found_intersection = False
end_x = check_fwd_a.x
end_y = check_fwd_a.y
for i in range(15, -15, -5):
if not found_intersection:
check_fwd_a.x = end_x + x_move * i * 0.001
check_fwd_a.y = end_y + y_move * i * 0.001
check_fwd_b.x = check_fwd_a.x + x_move * 0.01
check_fwd_b.y = check_fwd_a.y + y_move * 0.01
check_fwd_a.theta = parallel_theta
check_fwd_b.theta = perpendicular_theta
resp = check_srv(check_fwd_a, check_fwd_b)
if resp.valid:
new = Node(check_fwd_a.x, check_fwd_a.y,
check_fwd_a.theta, parallel_orientation,
parent, [1, 1, 1, 1])
new.update(goal, bonus)
node_list.append(new)
new2 = Node(check_fwd_b.x, check_fwd_b.y,
check_fwd_b.theta,
perpendicular_orientation, new,
end_explored)
new2.update(goal, bonus)
node_list.append(new2)
found_intersection = True
last_node.explored[direction] = -1
last_node.update(goal, bonus)
break
if check_ortho:
if direction == 0 or direction == 2:
if abs(check_plus_b.y -
parallel_check_goal) < 1.5 * resolution:
check_fwd_b.y = goal.y
check_fwd_a.y = check_fwd_b.y - y_move * 0.006
check_plus_a.y = goal.y
check_plus_b.y = goal.y
check_minus_a.y = goal.y
check_minus_b.y = goal.y
elif direction == 1 or direction == 3:
if abs(check_plus_b.x -
parallel_check_goal) < 1.5 * resolution:
check_fwd_b.x = goal.x
check_fwd_a.x = check_fwd_b.x - x_move * 0.006
check_plus_a.x = goal.x
check_plus_b.x = goal.x
check_minus_a.x = goal.x
check_minus_b.x = goal.x
check_ortho = False
ortho_intersect = True
resp_plus = check_srv(check_plus_a, check_plus_b)
resp_minus = check_srv(check_minus_a, check_minus_b)
if resp_plus.valid and resp_minus.valid:
new = Node(check_fwd_b.x, check_fwd_b.y, check_fwd_b.theta,
parallel_orientation, parent, mid_explored_both)
new.update(goal, bonus)
node_list.append(new)
last_node.explored[direction] = 1
last_node.update(goal, bonus)
last_node = new
last_check = True
elif resp_plus.valid:
new = Node(check_fwd_b.x, check_fwd_b.y, check_fwd_b.theta,
parallel_orientation, parent, mid_explored_plus)
new.update(goal, bonus)
node_list.append(new)
last_node.explored[direction] = 1
last_node.update(goal, bonus)
last_node = new
last_check = True
elif resp_minus.valid:
new = Node(check_fwd_b.x, check_fwd_b.y, check_fwd_b.theta,
parallel_orientation, parent, mid_explored_minus)
new.update(goal, bonus)
node_list.append(new)
last_node.explored[direction] = 1
last_node.update(goal, bonus)
last_node = new
last_check = True
elif last_check == True:
if check_ortho:
if direction == 0 and check_plus_a.y - goal.y > 3.0 * resolution:
break
if direction == 1 and check_plus_a.x - goal.x > 3.0 * resolution:
break
if direction == 2 and goal.y - check_plus_a.y > 3.0 * resolution:
break
if direction == 3 and goal.x - check_plus_a.x > 3.0 * resolution:
break
advance = 2.0 * resolution
check_fwd_a.x += x_move * advance
check_fwd_a.y += y_move * advance
check_fwd_b.x += x_move * advance
check_fwd_b.y += y_move * advance
check_plus_a.x += x_move * advance
check_plus_b.x += x_move * advance
check_minus_a.x += x_move * advance
check_minus_b.x += x_move * advance
check_plus_a.y += y_move * advance
check_plus_b.y += y_move * advance
check_minus_a.y += y_move * advance
check_minus_b.y += y_move * advance
last_check = False
else:
if check_ortho:
if direction == 0 and check_plus_a.y - goal.y > size * .25:
new = Node(check_fwd_b.x, check_fwd_b.y, check_fwd_b.theta,
parallel_orientation, parent, mid_explored_stop)
new.update(goal, bonus)
last_node.explored[direction] = 1
last_node.update(goal, bonus)
node_list.append(new)
break
if direction == 1 and check_plus_a.x - goal.x > size * .25:
new = Node(check_fwd_b.x, check_fwd_b.y, check_fwd_b.theta,
parallel_orientation, parent, mid_explored_stop)
new.update(goal, bonus)
last_node.explored[direction] = 1
last_node.update(goal, bonus)
node_list.append(new)
break
if direction == 2 and goal.y - check_plus_a.y > size * .25:
new = Node(check_fwd_b.x, check_fwd_b.y, check_fwd_b.theta,
parallel_orientation, parent, mid_explored_stop)
new.update(goal, bonus)
last_node.explored[direction] = 1
last_node.update(goal, bonus)
node_list.append(new)
break
if direction == 3 and goal.x - check_plus_a.x > size * .25:
new = Node(check_fwd_b.x, check_fwd_b.y, check_fwd_b.theta,
parallel_orientation, parent, mid_explored_stop)
new.update(goal, bonus)
last_node.explored[direction] = 1
last_node.update(goal, bonus)
node_list.append(new)
break
last_check = False
#rospy.loginfo("stopped at: %f, %f", check_fwd_b.x, check_fwd_b.y)
return None
import rospy
from geometry_msgs.msg import Pose2D
d_sample = 1.0 #takes care of drift
d_threshold = 0.1 #tolerance with the destination
dest = [(2.0, 0.0)] #destination coordinates
def get_source(data):
s_pose.x = data.x
s_pose.y = data.y
s_pose.theta = data.theta
s_pose = Pose2D()
s_pose.x = 0
s_pose.y = 0
s_pose.theta = 0
d_pose = Pose2D()
d_pose.x = 0
d_pose.y = 0
d_pose.theta = 0
#check for theta value
rospy.init_node('robot1_destinations')
rate = rospy.Rate(10)
dest_pose_p = rospy.Publisher('robot1/d_pose', Pose2D, queue_size=10)
source_pose_sub = rospy.Subscriber('robot1/s_pose', Pose2D, get_source)
time.sleep(3)
def explore_perpendicular(node_list, parent, direction, goal, resolution,
bonus, h_angle, v_angle, size, check_srv):
if direction == 0:
x_move = 0.0
y_move = 1.0
explored_dir = 2
new_orientation = 1
new_theta = v_angle
new_explored = [0, -1, 1, -1]
elif direction == 1:
x_move = 1.0
y_move = 0.0
explored_dir = 3
new_orientation = 0
new_theta = h_angle
new_explored = [-1, 0, -1, 1]
elif direction == 2:
x_move = 0.0
y_move = -1.0
explored_dir = 0
new_orientation = 1
new_theta = v_angle
new_explored = [1, -1, 0, -1]
elif direction == 3:
x_move = -1.0
y_move = 0.0
explored_dir = 1
new_orientation = 0
new_theta = h_angle
new_explored = [-1, 1, -1, 0]
check_a = Pose2D()
check_b = Pose2D()
check_a.x = parent.x
check_a.y = parent.y
check_a.theta = parent.theta
check_b.x = parent.x + 0.01 * x_move
check_b.y = parent.y + 0.01 * y_move
check_b.theta = new_theta
resp = check_srv(check_a, check_b)
if not resp.valid:
check_b.x = parent.x + 0.04 * x_move
check_b.y = parent.y + 0.04 * y_move
check_b.theta = new_theta
resp = check_srv(check_a, check_b)
if not resp.valid:
parent.explored[direction] = -1
parent.update(goal, bonus)
return None
parent.explored[direction] = 1
new = Node(check_b.x, check_b.y, new_theta, new_orientation, parent,
new_explored)
new.update(goal, bonus)
parent.update(goal, bonus)
for point in list(node_list):
if parent.distance(point) < 2.1 * resolution:
if (parent.explored == point.explored and point.start_dist > 0.001
) and (parent.parent == point.parent
and parent.orientation == point.orientation):
node_list.remove(point)
node_list.append(new)
if new.orientation == goal.orientation and new.distance(goal) < size * 0.1:
check_a.x = new.x
check_a.y = new.y
check_a.theta = new.theta
check_b.x = goal.x
check_b.y = goal.y
check_b.theta = goal.theta
resp = check_srv(check_a, check_b)
#rospy.loginfo("Goal check:" + repr(resp.valid))
if resp.valid:
goal.parent = new
return goal
return None
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64
from geometry_msgs.msg import Pose2D
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float64MultiArray
from std_msgs.msg import MultiArrayLayout
from std_msgs.msg import MultiArrayDimension
#Float64 for wheel velocities
rw_vel = 0.0
lw_vel = 0.0
#Pose2D (x, y, theta) for beacon location relative to robot location
beacon_found = False
beacon_pose = Pose2D()
beacon_pose.x = 0
beacon_pose.y = 0
beacon_pose.theta = -180
#LaserScan information on the robot laserscan taopic
laser_scan = LaserScan()
#creat the wheel-speed publisher
pub = rospy.Publisher(
'robot0/kinematic_params', Float64MultiArray, queue_size=10)
#Get the sensor reading on the left wheel velocity
def lw_callback(lw_data):
global lw_vel
def __init__(self,
attr_x=0,
attr_y=0,
n_obs=1,
obs_pos=[[3, 0]],
dsController=False,
freq=100,
n_intSteps=20,
dt_simu=0.1):
rospy.init_node('VelocityController', anonymous=True)
rospy.on_shutdown(self.call_shutdown) # shutdown command
rate = rospy.Rate(freq) # Frequency
self.dt = 1. / freq
attr = [float(attr_x), float(attr_y)]
self.dim = 2 # Dimensionaliyty of obstacle avoidance problem
self.n_obs = int(
n_obs) # number of obstacles, at the moment manually do automatic
print('self.pos_attr', self.n_obs)
self.awaitingPos = True
self.robo_pos = 0
# Create position subscriber
self.sub_pos = rospy.Subscriber("/Read_joint_state/quickie_state",
Pose2D, self.callback_pos)
self.sub_attr = rospy.Subscriber("/Attractor_position", Pose2D,
self.callback_attr)
# Create velocity publisher
self.pub_vel = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.pub_attr = rospy.Publisher('attractor',
PointStamped,
queue_size=10)
# Human motion limits
self.velHuman_max = 3.0 / 3.6
self.velHuman_lim = 5.0 / 3.6 # [m/s]
self.distLim_origin = 4 # [m]
# Attractor position
self.pos_attr = np.array(attr)
print('self.pos_attr', self.pos_attr)
# Robot and environment gemoetry
self.wheelRad = 0.155 # [m]
self.robo_dim = np.array([0.66, 0.78]) # [m]
# Initialize obstacle stuff
if self.n_obs > 0:
self.awaitingObs = [True] * self.n_obs
self.obs_pos = [0] * self.n_obs
self.pub_obs = [0] * self.n_obs
for oo in range(self.n_obs):
self.pub_obs[oo] = rospy.Publisher(
"/Read_obstacle_Position/Desired_Pose_sphere" +
str(oo + 1),
Pose2D,
queue_size=10)
self.obs_read = 0.5 # [m]
self.obs_dim = [0.5, 0.5] # [m]
# Initialize obstacles
if self.n_obs > 1: # Circular initial set up
R_obsPos = 7
for oo in range(self.n_obs):
self.obs_pos[oo] = Pose2D()
self.obs_pos[oo].x = R_obsPos * np.cos(
2 * pi / self.n_obs * oo)
self.obs_pos[oo].y = R_obsPos * np.sin(
2 * pi / self.n_obs * oo)
self.obs_pos[oo].theta = 0
self.pub_obs[oo].publish(self.obs_pos[oo])
if self.n_obs == 1:
for oo in range(self.n_obs):
self.obs_pos[oo] = Pose2D()
self.obs_pos[oo].x = obs_pos[oo][0]
self.obs_pos[oo].y = obs_pos[oo][1]
self.obs_pos[oo].theta = 0
self.pub_obs[oo].publish(self.obs_pos)
while (self.awaitingPos):
print('WAITING - missing robot position')
rospy.sleep(0.1)
if self.n_obs > 0:
# Create obstacles
sf_a = LA.norm(self.robo_dim / 2) * 1.05
self.obs = []
for oo in range(self.n_obs):
self.obs.append(
Obstacle(
a=[0.5 + sf_a, 0.5 + sf_a],
# a=[1.48,1.48],
# TODO : more exact safety margin
#sf = LA.norm(self.robo_dim)/np.min(self.obs_dim)+1,
sf=1,
th_r=self.obs_pos[oo].theta, # zero for the moment
p=1, # Circular obstacles
x0=[self.obs_pos[oo].x, self.obs_pos[oo].y],
xd=[0, 0]))
print('got obstacle {}'.format(oo))
# only for cirucla obstacles
self.dist_min = self.obs[0].a[0] * 2 + 0.01
#################### MAIN CONTROL LOOP ####################
print('Starting loop.')
# Prepare variables before loop
n_traj = 2
pos = np.zeros((self.dim, n_traj + 1))
ds = np.zeros((self.dim, n_traj))
while not rospy.is_shutdown():
if self.n_obs > 0:
# Update position and velocity
self.update_obsPosition()
# Prediction of obstacle
obs_pred = copy.deepcopy(self.obs)
pos[:, 0] = np.array([self.robo_pos.x, self.robo_pos.y])
# Initial velocity
for ii in range(n_traj):
ds_init = linearAttractor_const(pos[:, ii],
x0=self.pos_attr,
velConst=0.8)
if self.n_obs > 0:
ds_mod = obs_avoidance_interpolation(
pos[:, ii],
ds_init,
self.obs,
vel_lim=0.2,
attractor=self.pos_attr)
print('ds mod', ds_mod)
print('mag ds mod', LA.norm(ds_mod))
else:
ds_mod = ds_init
print('ds init', ds_mod)
ds[:, ii] = ds_mod
pos[:, ii + 1] = self.dt * ds_mod + pos[:, ii]
for oo in range(self.n_obs):
obs_pred[
oo].x0 = obs_pred[oo].x0 + obs_pred[oo].xd * self.dt
# Publish velocity
vel = Twist()
vel.linear.x = LA.norm(ds[:, 0]) / self.wheelRad
phi0 = self.robo_pos.theta
phi1 = np.arctan2(ds[1, 1], ds[0, 1])
dPhi = phi1 - phi0
# correct for disoncitnuity at -pi/pi
while dPhi > pi:
dPhi = pi - 2 * pi
while dPhi < -pi:
dPhi = pi + 2 * pi
dPhi = dPhi / self.dt
dPhi_max = 40. / 180 * pi
if np.abs(dPhi) > dPhi_max:
dPhi = np.copysign(dPhi_max, dPhi)
# vel.linear.x = 0
vel.linear.x = vel.linear.x / 50
print('WAIT -- repositioning')
vel.angular.z = dPhi * 0.8
if False:
vel.angular.z = 0
vel.linear.x = 0
self.pub_vel.publish(vel)
# Publish Attractor
attractor = PointStamped()
attractor.header.frame_id = 'gazebo_world'
attractor.point.x = self.pos_attr[0]
attractor.point.y = self.pos_attr[1]
attractor.point.z = 0.25
self.pub_attr.publish(attractor)
rate.sleep()
print('finished')
self.call_shutdown()
