def cbNextGoal(self, msg):
if not msg.data:
return
if self.fsm_state != "LANE_FOLLOWING":
return
# Pop next goal on path
try:
# If we can pop another goal
print "pop!"
self.goal = self.path.pop()
goal_msg = Pose2DStamped()
goal_msg.header.stamp = rospy.Time.now()
goal_msg.x = self.goal[0]
goal_msg.y = self.goal[1]
goal_msg.theta = self.goal[2]
self.pub_goal.publish(goal_msg)
#self.Rate.sleep()
except:
# Cannot pop another goal, reach destination
# Stop the robot and wait for coordination
goal_msg = Pose2DStamped()
goal_msg.header.stamp = rospy.Time.now()
goal_msg.x = 0.0
goal_msg.y = 0.0
goal_msg.theta = 0.0
self.pub_goal.publish(goal_msg)
debug = False
if debug:
self.path = [(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (1.0, 1.0, 0.0),
(0.0, 1.0, 0.0)]
else:
reach_msg = BoolStamped()
reach_msg.header.stamp = rospy.Time.now()
reach_msg.data = True
self.pub_reach_dest.publish(reach_msg)
def __init__(self):
# records the calculated pose of the robot
self.pose = Pose2DStamped()
# subscribes to wheels_driver_node/wheels_cmd
rospy.Subscriber("wheels_driver_node/wheels_cmd", WheelsCmdStamped, self.callback)
self.pub = rospy.Publisher("lab4_results", Pose2DStamped, queue_size=10)
def velocity_callback(self, msg_velocity):
if self.last_pose.header.stamp.to_sec() > 0: # skip first frame
delta_t = (msg_velocity.header.stamp -
self.last_pose.header.stamp).to_sec()
[theta_delta, x_delta,
y_delta] = self.integrate(self.last_theta_dot, self.last_v,
delta_t)
[theta_res, x_res,
y_res] = self.propagate(self.last_pose.theta, self.last_pose.x,
self.last_pose.y, theta_delta, x_delta,
y_delta)
self.last_pose.theta = theta_res
self.last_pose.x = x_res
self.last_pose.y = y_res
# Stuff the new pose into a message and publish
msg_pose = Pose2DStamped()
msg_pose.header = msg_velocity.header
msg_pose.header.frame_id = self.veh_name
msg_pose.theta = theta_res
msg_pose.x = x_res
msg_pose.y = y_res
self.pub_pose.publish(msg_pose)
self.last_pose.header.stamp = msg_velocity.header.stamp
self.last_theta_dot = msg_velocity.omega
self.last_v = msg_velocity.v
def __init__(self, node_name):
# Initialize the DTROS parent class
super(VelocityToPoseNode,
self).__init__(node_name=node_name,
node_type=NodeType.LOCALIZATION)
# Get the vehicle name
self.veh_name = rospy.get_namespace().strip("/")
# Keep track of the last known pose
self.last_pose = Pose2DStamped()
self.last_theta_dot = 0
self.last_v = 0
# Setup the publisher
self.pub_pose = rospy.Publisher("~pose",
Pose2DStamped,
queue_size=1,
dt_topic_type=TopicType.LOCALIZATION)
# Setup the subscriber
self.sub_velocity = rospy.Subscriber("~velocity",
Twist2DStamped,
self.velocity_callback,
queue_size=1)
# ---
self.log("Initialized.")
def __init__(self):
self.node_name = 'position_filter_node'
# Read parameters
self.veh_name = self.setupParameter("~veh_name", "megaman")
fi_theta_dot_function = self.setupParameter(
'~fi_theta_dot_function_param', 'Duty_fi_theta_dot_naive')
fi_v_function = self.setupParameter('~fi_v_function_param',
'Duty_fi_v_naive')
theta_dot_weights = matrix(
self.setupParameter('~theta_dot_weights_param', [-1.0]))
v_weights = matrix(self.setupParameter('~v_weights_param', [1.0]))
#Setup the forward kinematics model
self.fk = Forward_kinematics.Forward_kinematics(
fi_theta_dot_function, fi_v_function, matrix(theta_dot_weights),
matrix(v_weights))
#Setup the publisher and subscriber
self.sub_velocity = rospy.Subscriber("~velocity", Twist2DStamped,
self.velocityCallback)
self.pub_pose = rospy.Publisher("~pose", Pose2DStamped, queue_size=1)
#Keep track of the last known pose
self.last_pose = Pose2DStamped()
self.last_theta_dot = 0
self.last_v = 0
rospy.loginfo("[%s] has started", self.node_name)
def cbSwitch(self, msg):
self.active = msg.data
goal_msg = Pose2DStamped()
goal_msg.header.stamp = rospy.Time.now()
goal_msg.x = self.goal[0]
goal_msg.y = self.goal[1]
goal_msg.theta = self.goal[2]
self.pub_goal.publish(goal_msg)
def motion_model_callback(self, msg_velocity):
"""
This function will use robot velocity information to give a new state
Performs the calclulation from velocity to pose and publishes a messsage with the result.
Feel free to modify this however you wish. It's left more-or-less as-is
from the official duckietown repo
Args:
msg_velocity (:obj:`Twist2DStamped`): the current velocity message
"""
if self.last_pose.header.stamp.to_sec() > 0: # skip first frame
dt = (msg_velocity.header.stamp -
self.last_pose.header.stamp).to_sec()
# Integrate the relative movement between the last pose and the current
theta_delta = self.last_theta_dot * dt
# to ensure no division by zero for radius calculation:
if np.abs(self.last_theta_dot) < 0.000001:
# straight line
x_delta = self.last_v * dt
y_delta = 0
else:
# arc of circle
radius = self.last_v / self.last_theta_dot
x_delta = radius * np.sin(theta_delta)
y_delta = radius * (1.0 - np.cos(theta_delta))
# Add to the previous to get absolute pose relative to the starting position
theta_res = self.last_pose.theta + theta_delta
x_res = self.last_pose.x + x_delta * np.cos(
self.last_pose.theta) - y_delta * np.sin(self.last_pose.theta)
y_res = self.last_pose.y + y_delta * np.cos(
self.last_pose.theta) + x_delta * np.sin(self.last_pose.theta)
# Update the stored last pose
self.last_pose.theta = theta_res
self.last_pose.x = x_res
self.last_pose.y = y_res
# TODO Note how this puts the motion model estimate into a message and publishes the pose.
# You will also need to publish the pose coming from sensor fusion when you correct
# the estimates from the motion model
msg_pose = Pose2DStamped()
msg_pose.header = msg_velocity.header
msg_pose.header.frame_id = self.veh_name
msg_pose.theta = theta_res
msg_pose.x = x_res
msg_pose.y = y_res
self.pub_pose.publish(msg_pose)
self.last_pose.header.stamp = msg_velocity.header.stamp
self.last_theta_dot = msg_velocity.omega
self.last_v = msg_velocity.v
def __init__(self, outputDictQueue, logger):
try:
# Get the environment variables
self.ACQ_POSES_UPDATE_RATE = float(os.getenv('ACQ_POSES_UPDATE_RATE', 10)) #Hz
self.ACQ_ODOMETRY_UPDATE_RATE = float(os.getenv('ACQ_ODOMETRY_UPDATE_RATE', 30)) #Hz
self.ACQ_STATIONARY_ODOMETRY = bool(int(os.getenv('ACQ_STATIONARY_ODOMETRY', 0)))
self.ACQ_SOCKET_HOST = os.getenv('ACQ_SOCKET_HOST', '127.0.0.1')
self.ACQ_SOCKET_PORT = int(os.getenv('ACQ_SOCKET_PORT', 65432))
self.ACQ_DEVICE_NAME = os.getenv('ACQ_DEVICE_NAME', "watchtower10")
self.ACQ_TOPIC_RAW = os.getenv('ACQ_TOPIC_RAW', "camera_node/image/raw")
self.ACQ_TOPIC_CAMERAINFO = os.getenv('ACQ_TOPIC_CAMERAINFO', "camera_node/camera_info")
self.ACQ_TOPIC_VELOCITY_TO_POSE = os.getenv('ACQ_TOPIC_VELOCITY_TO_POSE', None)
self.ACQ_TEST_STREAM = bool(int(os.getenv('ACQ_TEST_STREAM', 1)))
self.ACQ_BEAUTIFY = bool(int(os.getenv('ACQ_BEAUTIFY', 1)))
self.ACQ_TAG_SIZE = float(os.getenv('ACQ_TAG_SIZE', 0.065))
# Initialize ROS nodes and subscribe to topics
rospy.init_node('acquisition_processor', anonymous=True, disable_signals=True)
self.subscriberRawImage = rospy.Subscriber("/"+self.ACQ_DEVICE_NAME+"/"+self.ACQ_TOPIC_RAW, CompressedImage,
self.camera_image_callback, queue_size = 1)
self.subscriberCameraInfo = rospy.Subscriber("/"+self.ACQ_DEVICE_NAME+"/"+self.ACQ_TOPIC_CAMERAINFO, CameraInfo,
self.camera_info_callback, queue_size = 1)
if self.ACQ_TOPIC_VELOCITY_TO_POSE and self.ACQ_ODOMETRY_UPDATE_RATE>0: #Only if set (probably not for watchtowers)
self.subscriberCameraInfo = rospy.Subscriber("/"+self.ACQ_DEVICE_NAME+"/"+self.ACQ_TOPIC_VELOCITY_TO_POSE, Pose2DStamped,
self.odometry_callback, queue_size = 1)
# CvBridge is neccessary for the image processing
self.bridge = CvBridge()
# Initialize atributes
self.lastCameraInfo = None
self.lastCameraImage = None
self.lastImageProcessed = False
self.previousOdometry = Pose2DStamped()
self.previousOdometry.x = 0.0
self.previousOdometry.y = 0.0
self.previousOdometry.theta = 0.0
self.timeLastPub_odometry = rospy.get_time()
self.timeLastPub_poses = rospy.get_time()
self.outputDictQueue = outputDictQueue
self.logger = logger
dsp_options={"beautify": self.ACQ_BEAUTIFY, "tag_size": self.ACQ_TAG_SIZE}
self.dsp = deviceSideProcessor(dsp_options, self.logger)
except Exception as e:
self.logger.warning("acquisitionProcessor initialization failed: : %s" % str(e))
raise Exception("acquisitionProcessor initialization failed: : %s" % str(e))
def velocity_callback(self, msg_velocity):
"""
Performs the calclulation from velocity to pose and publishes a messsage with the result.
Args:
msg_velocity (:obj:`Twist2DStamped`): the current velocity message
"""
if self.last_pose.header.stamp.to_sec() > 0: # skip first frame
dt = (msg_velocity.header.stamp -
self.last_pose.header.stamp).to_sec()
# Integrate the relative movement between the last pose and the current
theta_delta = self.last_theta_dot * dt
# to ensure no division by zero for radius calculation:
if np.abs(self.last_theta_dot) < 0.000001:
# straight line
x_delta = self.last_v * dt
y_delta = 0
else:
# arc of circle
radius = self.last_v / self.last_theta_dot
x_delta = radius * np.sin(theta_delta)
y_delta = radius * (1.0 - np.cos(theta_delta))
# Add to the previous to get absolute pose relative to the starting position
theta_res = self.last_pose.theta + theta_delta
x_res = self.last_pose.x + x_delta * np.cos(
self.last_pose.theta) - y_delta * np.sin(self.last_pose.theta)
y_res = self.last_pose.y + y_delta * np.cos(
self.last_pose.theta) + x_delta * np.sin(self.last_pose.theta)
# Update the stored last pose
self.last_pose.theta = theta_res
self.last_pose.x = x_res
self.last_pose.y = y_res
# Stuff the new pose into a message and publish
msg_pose = Pose2DStamped()
msg_pose.header = msg_velocity.header
msg_pose.header.frame_id = self.veh_name
msg_pose.theta = theta_res
msg_pose.x = x_res
msg_pose.y = y_res
self.pub_pose.publish(msg_pose)
self.last_pose.header.stamp = msg_velocity.header.stamp
self.last_theta_dot = msg_velocity.omega
self.last_v = msg_velocity.v
def initial_pose(self):
distance = self.add_distance / self.count
x = self.add_x / self.count
y = self.add_y / self.count
angle_l = self.add_angle_l / self.count
self.count = 0
pose_init_msg = Pose2DStamped()
pose_init_msg.x = x
pose_init_msg.y = y
pose_init_msg.theta = angle_l
self.pub_pose.publish(pose_init_msg)
self.needed = False
ready = BoolStamped()
ready.header.stamp = rospy.Time.now()
ready.data = True
self.pub_detection.publish(ready)
def detection(self, msg):
cv_img = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8")
cv_crop = cv_img[np.floor(cv_img.shape[0]*.35):cv_img.shape[0],0:cv_img.shape[1]]
img_hsv = cv2.cvtColor(cv_crop, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(img_hsv, self.low_range, self.high_range)
params = cv2.SimpleBlobDetector_Params()
params.filterByColor = False
params.filterByArea = True
params.minArea = 120
params.filterByInertia = True
params.minInertiaRatio = 0.7
params.filterByConvexity = False
params.filterByCircularity = False
dectector = cv2.SimpleBlobDetector_create(params)
keypoints = dectector.detect(mask)
if len(keypoints) > 0:
self.count += 1
if self.time is not None:
self.time = rospy.get_time()
if self.count > 3:
detect_msg = BoolStamped()
detect_msg.header.stamp = rospy.Time.now()
detect_msg.data = True
pose_msg = Pose2DStamped()
pose_msg.header.stamp = rospy.Time.now()
pose_msg.x = keypoints[0].pt[0]
pose_msg.y = keypoints[0].pt[1]
pose_msg.theta = 0
if (pose_msg.x > 30 and pose_msg.y < 110) and self.detect:
self.duckie_detected.publish(detect_msg)
self.duckie_pose.publish(pose_msg)
self.detect = False
if self.time is not None:
if (rospy.get_time() - self.time) > 1:
self.count = 0
img_keypoints = cv2.drawKeypoints(cv_crop, keypoints, cv_crop, color=(0,0,255), flags=cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
img_keypoints_out = self.bridge.cv2_to_imgmsg(cv_crop, "bgr8")
img_mask_out = self.bridge.cv2_to_imgmsg(mask, "mono8")
self.detection_image.publish(img_keypoints_out)
self.mask.publish(img_mask_out)
def __init__(self):
# Get node name and vehicle name
self.node_name = rospy.get_name()
self.veh_name = self.node_name.split("/")[1]
# Keep track of the last known pose
self.last_pose = Pose2DStamped()
self.last_theta_dot = 0
self.last_v = 0
# Setup the publisher and subscriber
self.sub_velocity = rospy.Subscriber("~velocity",
Twist2DStamped,
self.velocity_callback,
queue_size=1)
self.pub_pose = rospy.Publisher("~pose", Pose2DStamped, queue_size=1)
rospy.loginfo("[%s] Initialized.", self.node_name)
def __init__(self):
#dimensions of image used by detection node
self.IMAGE_WIDTH = 256.0
self.IMAGE_HEIGHT = 67.0
#cmd velocities
self.omega = 8.0
self.v = 0.5
self.steering_enabled = False
self.last_start = BoolStamped()
self.last_pose = Pose2DStamped()
#subscribers
rospy.Subscriber("remote_steering_start", BoolStamped, self.start_steering_cb)
rospy.Subscriber("pose", Pose2DStamped, self.pose_cb)
#publishsers
self.pub_done = rospy.Publisher("remote_steering_complete", BoolStamped, queue_size=1)
self.pub_car_cmd = rospy.Publisher("lane_controller_node/car_cmd", Twist2DStamped, queue_size=1)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(SensorFusionNode, self).__init__(node_name=node_name,
node_type=NodeType.LOCALIZATION)
# Get the vehicle name
self.veh_name = rospy.get_namespace().strip("/")
# Keep track of the last known pose
self.last_pose = Pose2DStamped()
self.last_theta_dot = 0
self.last_v = 0
# TODO Feel free to use a flag like this to set which type of filter you're currently using.
# You can also define these as parameters in the launch file for this node if you're feeling fancy
# (or if this was a system you wanted to use in the real world), but a hardcoded flag works just as well
# for a class project like this.
self.FUSION_TYPE = "EKF"
# Setup the publisher
self.pub_pose = rospy.Publisher("~pose",
Pose2DStamped,
queue_size=1,
dt_topic_type=TopicType.LOCALIZATION)
# Setup the subscriber to the motion of the robot
self.sub_velocity = rospy.Subscriber(
f"/{self.veh_name}/kinematics_node/velocity",
Twist2DStamped,
self.motion_model_callback,
queue_size=1)
# Setup the subscriber for when sensor readings come in
self.sub_sensor = rospy.Subscriber(f"/{self.veh_name}/detected_tags",
Int32MultiArray,
self.sensor_fusion_callback,
queue_size=1)
# ---
self.log("Initialized.")
def set_task(self):
path_msg = Pose2DList()
move_msg = String()
# Publish move message
move_msg.data = self.current_task['move']
self.pub_set_move.publish(move_msg)
# Publish path messgae
data_list = []
for i in range(len(self.current_task['path'])):
# ith position
data = self.current_task['path'][i]
ith_pose = Pose2DStamped()
ith_pose.header.stamp = rospy.Time.now()
ith_pose.x = data[0]
ith_pose.y = data[1]
ith_pose.theta = data[2]
data_list.append(ith_pose)
path_msg.data_list = data_list
self.pub_set_path.publish(path_msg)
rospy.loginfo("[%s] set move and path to planners" %(self.node_name))
#!/usr/bin/env python
import rospy
import math
import csv
import sys
from duckietown_msgs.msg import Twist2DStamped, Pose2DStamped
from wifianalyzer_teamgrapes import wifiutils as wu
lastLinVel = 0.0
lastAngVel = 0.0
oldPose = Pose2DStamped()
f = open('./heatmapdata.csv', 'wt')
writer = csv.writer(f)
# integrates the linear and angular velocities to calculate displacement
def integrate(theta_dot, v, dt):
# change in theta
theta_delta = theta_dot * dt
# if the angular velocity is very small
# assume we are moving in a straight line
if abs(theta_dot) < 0.00001:
x_delta = v * dt
y_delta = 0
else:
# otherwise we are moving along the arc of a circle
r = v / theta_dot
x_delta = r * math.sin(theta_delta)
y_delta = r * (1.0 - math.cos(theta_delta))
def __init__(self, *args):
super(TestPositionFilterNode, self).__init__(*args)
self.pose = Pose2DStamped()
self.pose_prev = Pose2DStamped()
self.msg_received = False
rospy.init_node("nav_home")
rospack = rospkg.RosPack()
path = rospack.get_path('ank_ris')
steering_pub = rospy.Publisher('/lead/joy', Joy, queue_size=10, latch=True)
while steering_pub.get_num_connections() < 1 and not rospy.is_shutdown():
pass
command = Joy()
i = 0
while i < 8:
command.axes.append(0)
i += 1
home = Pose2DStamped()
current_pose = Pose2DStamped()
return_home = False
new_home = True
lin_speed = 0.5
ang_speed = 1
full_circle = 25
full_circle_time = 2
def triggerOdom():
command.axes[3] = 0.1
steering_pub.publish(command)
rospy.sleep(0.3)
command.axes[3] = 0
steering_pub.publish(command)
