def reports_callback(self, msg):
nowTime = rospy.Time.now()
try:
self.prev_state = self.state
except AttributeError as ae:
self.prev_state = msg.state
self.state = msg.state
self.robot_x = msg.state.position_x
self.robot_y = msg.state.position_y
self.robot_h = msg.state.orientation_angle
self.robot_v = 0
self.robot_w = 0
inc_t = (nowTime - self.prev_time).to_sec()
inc_x = self.state.position_x - self.prev_state.position_x
inc_y = self.state.position_y - self.prev_state.position_y
inc_r = np.sqrt((inc_x**2) + (inc_y**2))
self.robot_v = inc_r / inc_t
inc_h = self.state.orientation_angle - self.prev_state.orientation_angle
self.robot_w = inc_h / inc_t
rospy.logdebug_throttle(
5, "Node [" + rospy.get_name() + "] " + "Robot status: Pose ( " +
str(self.state.position_x) + ", " + str(self.state.position_y) +
", " + str(self.state.orientation_angle * 180.0 / np.pi) +
" deg), " + "Speed ( " + str(self.robot_v) + " m/sec, " +
str(self.robot_w * 180.0 / np.pi) + " deg/sec) ")
self.prev_time = nowTime
def closest_waypoint_ahead(self, pos_x, pos_y, yaw, waypoints):
''' Return index of closest point ahead '''
# Create some logging info
loginfo = 'yaw: {} | x: {} | y: {}'.format(yaw, pos_x, pos_y)
# Define unit vector for car orientation in global (x, y) coordinates
orient_x, orient_y = math.cos(yaw), math.sin(yaw)
# Filter waypoints to keep only the ones ahead of us by checking scalar product
waypoints_ahead = [
(n, wp) for (n, wp) in enumerate(waypoints)
if (orient_x * (wp.pose.pose.position.x - pos_x) + orient_y *
(wp.pose.pose.position.y - pos_y)) > 0
]
if not len(waypoints_ahead):
rospy.logwarn("No points detected ahead of us")
# Extract closest waypoint
closest_waypoint = min(
waypoints_ahead,
key=lambda wpidx:
(wpidx[1].pose.pose.position.x - self.ego_pos.position.x)**2 +
(wpidx[1].pose.pose.position.y - self.ego_pos.position.y)**2)
closest_index = closest_waypoint[0]
loginfo += '| Closest waypoint index: {}'.format(closest_index)
rospy.logdebug_throttle(0.1, loginfo)
return closest_index
def check_obstacle(self):
obstacle = ['no_obstacle', 0, 0]
try:
urgent = [(1 if x < u else 0)
for x, u in zip(self.ir_array_filtered, URGENT_BD)]
if (1 in urgent):
obstacle[0] = 'urgent'
if (1 in urgent[0:NUM_SS_FRONT]):
rospy.loginfo("Urgent front! Front: {}".format(
self._list(self.ir_front)))
obstacle[1] = 1
if (1 in urgent[NUM_SS_FRONT:NUM_SENSOR]):
rospy.loginfo("Urgent back! Back: {}".format(
self._list(self.ir_back)))
obstacle[2] = 1
return obstacle
else:
rospy.logdebug_throttle(1,
self.bubble_boundary[0:NUM_SS_FRONT])
# Xac dinh vi tri co vat can
self.obstacle_located = [(1 if x < bb else 0) for x,bb in \
zip(self.ir_front, \
self.bubble_boundary[0:NUM_SS_FRONT])]
if (1 in self.obstacle_located):
rospy.logdebug("obstacle_locate: {}".format(
self.obstacle_located))
obstacle[0] = 'bubble_obstacle'
obstacle[1] = 1
# return obstacle
except Exception as e:
rospy.logerr(e)
finally:
return obstacle
def read_encoder_data(self):
try:
struct_len = ord(self.serial_conn.read())
data_str = self.serial_conn.read(struct_len)
rec_checksum = ord(self.serial_conn.read())
except serial.SerialTimeoutException:
rospy.logerr("Serial timeout awaiting encoder data")
return False
# Use the Python struct library to interpret the data
# '<' enforces little-endianness
# L[n]l means:
# L - one unsigned long (timestamp)
# [n]l - [n] signed longs (encoder counts)
try:
format_str = "<L{}l".format(NUM_ENCODERS)
encoder_data = struct.unpack(format_str, data_str)
except struct.error:
rospy.logerr("read_encoder_data error in struct unpack. Message: %s", data_str)
return False
if rec_checksum == calc_checksum(data_str):
self.encoder_callback(encoder_data)
rospy.logdebug_throttle(2, "encoder message received")
else:
rospy.logerr("Encoder checksum failure, ignoring message")
def control(self, cur_linear, cur_ang, target_linear, target_ang, dt):
speed_error = target_linear.x - cur_linear.x
brake = 0.0
throttle = 0.0
acc = self.acc_pid.step(speed_error, dt)
if speed_error < 0.0:
# http://www.asawicki.info/Mirror/Car%20Physics%20for%20Games/Car%20Physics%20for%20Games.html
# torque = force * wheel_radius = mass * acceleration * wheel_radius
torque = abs(acc) * self.vehicle_mass * self.wheel_radius
brake = torque
else:
throttle = acc
if throttle < 0.01:
throttle = 0.0
steer = self.steering_controller.get_steering(target_linear.x,
target_ang.z,
cur_linear.x)
rospy.logdebug_throttle(
2, 'cur {}, target {}, throttle {}, brake {}, steer {}'.format(
cur_linear.x, target_linear.x, throttle, brake, steer))
return throttle, brake, steer
def send_commands(self, command_struct):
"""
Writes the desired commands in command_struct to the device
over serial. The command_struct *must* be the exact same as
the struct to receive it on the other device!
Currently, it is 10 bytes:
lin_vel, ang_vel, base_rotate, actuator_1_move,
actuator_2_move, wrist_rotate, wrist_actuator_move, gripper_move
servo_yaw, servo_pitch
A command to control the camera servo will most likely be added
at some point, probably another byte
"""
rospy.logdebug_throttle(2, "sending commands: {}".format(" ".join(str(c) for c in command_struct)))
assert(len(command_struct) == COMMAND_STRUCT_LEN)
command_data = struct.pack(COMMAND_STRUCT_FORMAT, *command_struct)
checksum = calc_checksum(command_data)
try:
self.serial_conn.write(BEGIN_MESSAGE_BYTE)
# Send struct length - must convert len(command_data) to byte array
self.serial_conn.write(bytearray((len(command_data),)))
self.serial_conn.write(command_data)
# Send checksum
self.serial_conn.write(bytearray((checksum,))) # Regularly times out here!
except serial.SerialTimeoutException:
rospy.logerr("Serial write timeout")
# Just pass for now, ignoring this attempt and trying again later
pass
def callback(self,data):
rospy.logdebug("reached callback. that means I can read the Subscriber!")
rospy.set_param('~alive',1)
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
scores = self.net.predict_single_frame([cv_image,], 'fc-action', frame_size=(self.framesize_width, self.framesize_height))
#print((scores))
#this publishes the instant time version, aka, per frame
self.label_pub.pub([scores])
rospy.logdebug("published the label for instant time version!")
#this part publishes the frame_window version
self.frame_scores.append(scores)
if len(self.frame_scores)>self.classwindow:
self.frame_scores.pop(0)
self.label_fw_pub.pub(self.frame_scores)
rospy.logdebug("published the label for the frame window version!")
self.lock.acquire()
if self.startedownvid:
self.ownvidscores.append(scores)
else:
rospy.logdebug_throttle(20,"waiting for start_vidscores call to start classifying ownvid")
self.lock.release()
def forward_kinematics(self):
"""
Calculate current twist of the rover given current drive and corner motor velocities
Also approximate current turning radius.
Note that forward kinematics means solving an overconstrained system since the corner
motors may not be aligned perfectly and drive velocities might fight each other
"""
# calculate current turning radius according to each corner wheel's angle
# corner motor angles should be flipped since different coordinate axes in this node (positive z up)
theta_fl = -self.curr_positions['corner_left_front']
theta_fr = -self.curr_positions['corner_right_front']
theta_bl = -self.curr_positions['corner_left_back']
theta_br = -self.curr_positions['corner_right_back']
# sum wheel angles to find out which direction the rover is mostly turning in
if theta_fl + theta_fr + theta_bl + theta_br > 0: # turning left
r_front_closest = self.d1 + self.angle_to_turning_radius(theta_fl)
r_front_farthest = -self.d1 + self.angle_to_turning_radius(theta_fr)
r_back_closest = -self.d1 - self.angle_to_turning_radius(theta_bl)
r_back_farthest = self.d1 - self.angle_to_turning_radius(theta_br)
else: # turning right
r_front_farthest = self.d1 + self.angle_to_turning_radius(theta_fl)
r_front_closest = -self.d1 + self.angle_to_turning_radius(theta_fr)
r_back_farthest = -self.d1 - self.angle_to_turning_radius(theta_bl)
r_back_closest = self.d1 - self.angle_to_turning_radius(theta_br)
# get a best estimate of the turning radius by taking the median value (avg sensitive to outliers)
approx_turning_radius = sum(sorted([r_front_farthest, r_front_closest, r_back_farthest, r_back_closest])[1:3])/2.0
if math.isnan(approx_turning_radius):
approx_turning_radius = self.max_radius
rospy.logdebug_throttle(1, "Current approximate turning radius: {}".format(round(approx_turning_radius, 2)))
self.curr_turning_radius = approx_turning_radius
def execute(self, userdata=None):
goal_pose = self.goal_pose.resolve() if hasattr(
self.goal_pose, "resolve") else self.goal_pose
if self._goal_reached(
*self._get_target_delta_in_robot_frame(goal_pose)):
rospy.loginfo("We are already there")
return 'succeeded'
rate = rospy.Rate(self._rate)
rospy.loginfo("Starting alignment ....")
while not rospy.is_shutdown():
dx, dy, dyaw = self._get_target_delta_in_robot_frame(goal_pose)
if self._goal_reached(dx, dy, dyaw):
rospy.loginfo("Goal reached")
return 'succeeded'
rospy.logdebug_throttle(
0.1, "Aligning .. Delta = {} {} {}".format(dx, dy, dyaw))
self.robot.base.force_drive(
vx=_clamp(self.params.abs_vx, self.params.position_gain * dx),
vy=_clamp(self.params.abs_vy, self.params.position_gain * dy),
vth=_clamp(self.params.abs_vyaw,
self.params.rotation_gain * dyaw),
timeout=1 / float(self._rate),
loop_rate=self._rate,
stop=False)
rate.sleep()
self.robot.base.force_drive(vx=0, vy=0, vth=0, timeout=0, stop=True)
return 'failed'
def callback(data):
r = numpy.array(data.ranges)
# x2 stuff because LaserScan has 721 increments ~= 0.5 deg resolution
back = numpy.concatenate((r[:0 + pan], r[360 * 2 - pan:]))
right = r[90 * 2 - pan:90 * 2 + pan]
front = r[180 * 2 - pan:180 * 2 + pan]
left = r[270 * 2 - pan:270 * 2 + pan]
dists = dict(
back=back[numpy.nonzero(back)].mean(),
right=right[numpy.nonzero(right)].mean(),
front=front[numpy.nonzero(front)].mean(),
left=left[numpy.nonzero(left)].mean(),
)
rospy.logdebug_throttle(1, "distances\n%r" % dists)
if dists[side] > door_dist:
self.logger.info("open detected! dist to %r: %.3r m" %
(side, dists[side]))
self.cob.unregister()
self.res = "open"
return
else:
#self.res = "closed"
self.logger.debug("dist to %r: %.3r m" % (side, dists[side]))
pass
def _spin_once(self):
"""
one optimisation iteration, without rate control
checks that required data are here, and calls for planification, then publishes results
"""
with self.orders_lock:
with self.map_lock:
if self.orders is None:
rospy.logwarn_throttle(
10, 'Captain cannot plan path : missing orders')
return False, None
if self.map is None:
rospy.logwarn_throttle(
10, 'Captain cannot plan path : missing map')
return False, None
if self.last_orders_seq == self.orders.header.seq:
# orders didn't change, no need to work
rospy.logdebug_throttle(1, "Captain didn't get new orders")
return False, None
rospy.loginfo_throttle(1, "starting planification")
# let's compute !
success, results = self._planif_assign()
self.last_orders_seq = self.orders.header.seq
if not success:
rospy.logwarn_throttle(1, "RRT search did not succeed")
else:
self._publish_captain_orders(results)
self._publish_captain_viz(results)
def publishQTCdata(self):
# this method is called by one of the other callbacks, so its under the lock already
#with self.data_lock:
validData = self.closest_human_pose and self.closest_human_twist and self.robotPoseSt
if validData:
# all should be already in global frame ...
(xh0, yh0, hh0, vh0, wh0) = self.getHumanState()
(xh1, yh1, hh1) = self.getNextHumanState(xh0, yh0, hh0, vh0, wh0)
xr0 = self.robotPoseSt.pose.position.x
yr0 = self.robotPoseSt.pose.position.y
hr0 = self.getRotation(self.robotPoseSt.pose.orientation)
(xr1, yr1, hr1) = self.getNextRobotState()
# get state only if transforms where successfull
if ((xh0!=None) and (xh1!=None) and(xr0!=None) and(xr1!=None)):
(isValid, state) = self.getQSRState(xh0, yh0, xh1, yh1, xr0, yr0, xr1, yr1)
self.qtc_state = state
self.is_valid = isValid
# Finally publish .......................
if self.is_valid:
self.qtc_state_pub.publish(self.qtc_state)
qtc_data = Float64MultiArray()
qtc_data.data = [xh0, yh0, hh0, xh1, yh1, hh1, xr0, yr0, hr0, xr1, yr1, hr1]
self.qtc_points_pub.publish(qtc_data)
rospy.logdebug_throttle(1, "Node [" + rospy.get_name() + "] " +
"Updated visuals ... "
)
def calculate_cp(self, com_mark):
current_time = com_mark.header.stamp
time_difference = (current_time - self.prev_t).to_sec()
if current_time is not self.prev_t:
x_dot = (com_mark.pose.position.x - self.prev_x) / time_difference
y_dot = (com_mark.pose.position.y - self.prev_y) / time_difference
try:
trans = self.tf_buffer.lookup_transform(
'world', self.foot_link, rospy.Time())
try:
multiplier = sqrt(com_mark.pose.position.z / self.g)
except ValueError:
rospy.logdebug_throttle(
1,
'Cannot calculate capture point, because center of mass height is '
'smaller than 0')
return self.marker
x_cp = trans.transform.translation.x + x_dot * multiplier
y_cp = trans.transform.translation.y + y_dot * multiplier
self.update_marker(x_cp, y_cp)
self.prev_x = com_mark.pose.position.x
self.prev_y = com_mark.pose.position.y
self.prev_t = current_time
except tf2_ros.TransformException as e:
rospy.logdebug(
'Error in trying to lookup transform for capture point: {error}'
.format(error=e))
return self.marker
def reports_callback(self, msg):
with self.data_lock:
nowTime = rospy.Time.now()
self.prev_state = self.state
self.state = msg.state
self.robot_x = msg.state.position_x
self.robot_y = msg.state.position_y
self.robot_h = msg.state.orientation_angle
self.robot_v = 0
self.robot_w = 0
if (self.prev_state and self.prev_time):
inc_t = (nowTime - self.prev_time).to_sec()
inc_x = self.state.position_x - self.prev_state.position_x
inc_y = self.state.position_y - self.prev_state.position_y
inc_r = np.sqrt((inc_x**2) + (inc_y**2))
inc_h = self.state.orientation_angle - self.prev_state.orientation_angle
if (inc_t > 0):
self.robot_v = inc_r / inc_t
self.robot_w = inc_h / inc_t
else:
self.robot_v = 0
self.robot_w = 0
rospy.logdebug_throttle(
5, "Node [" + rospy.get_name() + "] " +
"Robot status: Pose ( " + str(self.state.position_x) +
", " + str(self.state.position_y) + ", " +
str(self.state.orientation_angle * 180.0 / np.pi) +
" deg), " + "Speed ( " + str(self.robot_v) + " m/sec, " +
str(self.robot_w * 180.0 / np.pi) + " deg/sec) ")
self.prev_time = nowTime
def getNextRobotState(self):
xr1 = yr1 = hr1 = None
inc_x = inc_y = inc_h = dt = 0
if (self.prev_robotPoseSt and self.robotPoseSt and (self.robotPose_time > self.prev_time) ):
t = [1, 2, 3]
p = [3, 2, 0]
np.interp(2.5, t, p)
inc_x = self.robotPoseSt.pose.position.x - self.prev_robotPoseSt.pose.position.x
inc_y = self.robotPoseSt.pose.position.y - self.prev_robotPoseSt.pose.position.y
inc_h = self.getRotation(self.robotPoseSt.pose.orientation) - self.getRotation(self.prev_robotPoseSt.pose.orientation)
dt = (self.sampling_time ) / (self.robotPose_time - self.prev_time).to_sec()
if self.robotPoseSt:
xr1 = self.robotPoseSt.pose.position.x + inc_x*dt
yr1 = self.robotPoseSt.pose.position.y + inc_y*dt
hr1 = self.getRotation(self.robotPoseSt.pose.orientation) + inc_h*dt
rospy.logdebug_throttle(5, "Node [" + rospy.get_name() + "] " + "\n " +
"Robot status: Pose 0 ( " +
'{0:.2f}'.format(self.prev_robotPoseSt.pose.position.x) + ", " + '{0:.2f}'.format(self.prev_robotPoseSt.pose.position.y) + ", " +
'{0:.2f}'.format(self.getRotation(self.prev_robotPoseSt.pose.orientation) *180.0/np.pi) + " deg) " +
" at " + '{0:.2f}'.format(self.prev_time.to_sec() ) + " secs." + "\n" +
"Robot status: Pose 1 ( " +
'{0:.2f}'.format(self.robotPoseSt.pose.position.x) + ", " + '{0:.2f}'.format(self.robotPoseSt.pose.position.y) + ", " +
'{0:.2f}'.format(self.getRotation(self.robotPoseSt.pose.orientation) *180.0/np.pi) + " deg) " +
" at " + '{0:.2f}'.format(self.robotPose_time.to_sec() ) + " secs." + "\n" +
"Robot status: Pose 2( " +
'{0:.2f}'.format(xr1) + ", " + '{0:.2f}'.format(yr1) + ", " +
'{0:.2f}'.format(hr1 *180.0/np.pi) + " deg) " +
" at " + '{0:.2f}'.format(self.sampling_time+self.robotPose_time.to_sec() ) + " secs." + "\n"
)
return (xr1, yr1, hr1)
def execute(self, ud):
if self._goal_reached(*self._get_target_delta_in_robot_frame(self.goal_pose)):
rospy.loginfo("We are already there")
return
rospy.loginfo("Starting alignment ....")
while not rospy.is_shutdown():
dx, dy, dyaw = self._get_target_delta_in_robot_frame(self.goal_pose)
if self._goal_reached(dx, dy, dyaw):
break
rospy.logdebug_throttle(0.1, "Aligning .. Delta = {} {} {}".format(dx, dy, dyaw))
self._cmd_vel_publisher.publish(Twist(
linear=Vector3(
x=_clamp(self.params.abs_vx, self.params.position_gain * dx),
y=_clamp(self.params.abs_vy, self.params.position_gain * dy)
),
angular=Vector3(z=_clamp(self.params.abs_vyaw, self.params.rotation_gain * dyaw))
))
self._rate.sleep()
rospy.loginfo("Goal reached")
def updateVisuals(self):
validData = self.qtc_data and self.qtc_state
if validData:
(xh0, yh0, hh0, xh1, yh1, hh1, xr0, yr0, hr0, xr1, yr1,
hr1) = self.qtc_data
tx = (xh0 + xr0) / 2.0
ty = (yh0 + yr0) / 2.0
data = MarkerArray()
# connecting line (blue) ..............................................
line = self.getMarker(Marker.LINE_STRIP, xh0, yh0, xr0, yr0, 0, 0,
1, "connecting_line")
data.markers.append(line)
# robot motion (red) .......................................................
line = self.getMarker(Marker.ARROW, xr0, yr0, xr1, yr1, 1, 0, 0,
"robot_motion")
data.markers.append(line)
# human motion (green) .......................................................
line = self.getMarker(Marker.ARROW, xh0, yh0, xh1, yh1, 0, 1, 0,
"human_motion")
data.markers.append(line)
# mfc should these lines be longer?
# descriptive text .......................................................
text = Marker()
text.id = 1
text.type = Marker.TEXT_VIEW_FACING
text.header.frame_id = self.global_frame_id
text.header.stamp = rospy.Time.now()
text.ns = "descriptor"
text.action = Marker.ADD
text.pose.orientation.w = 1.0
text.pose.position.x = tx
text.pose.position.y = ty
text.pose.position.z = 1 + 0.2
# TEXT_VIEW_FACING markers use only the z component of scale, specifies the height of an uppercase "A".
text.scale.z = 0.45
# we show next state as text
text.text = (self.qtc_state.data)
if (self.use_pretty_text):
text.text = self.getPrettyText(text.text)
# red color
text.color.r = 1
text.color.g = 0.0
text.color.a = 1.0
data.markers.append(text)
# Finally publish .......................
self.visual_pub.publish(data)
rospy.logdebug_throttle(
1, "Node [" + rospy.get_name() + "] " + "Updated visuals ... ")
def process_program_argument(self, int16_msg):
try:
self.program_argument = int16_msg
rospy.logdebug_throttle(5, "process_program_argument: [%d]",
int16_msg.data)
except Exception as e:
rospy.logerr('[script_executor:process_program_id] Exception: %s',
str(e))
def _controlCallback(self, msg):
self.control_msg = msg
err_x = self.control_msg.x.err_p
err_y = self.control_msg.y.err_p
err_z = self.control_msg.z.err_p
err_yaw = self.control_msg.yaw.err_p
err_xy = math.sqrt(err_x * err_x + err_y * err_y)
rospy.logdebug_throttle(
1, 'errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]' %
(err_xy, err_x, err_y, err_z, err_yaw))
def callback(self,data):
rospy.logdebug("reached callback. that means I can read the Subscriber!")
rospy.set_param('~alive',1)
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
#print(e)
rospy.logerr(e)
#since I am not using stacks for rgb images, I can prevent from making the rgb version any slower by using an if statement here
if self.rgbOrFlow == 'flow':
## and I want the combined flow version here, don't I? so I need to strip the frame apart into components. I think it is better than
#self.cv_image_stack.append(cv_image)
#this would be incorrect, i need to get each matrix and put it together in a stack. first x then y
# from ros_flow_bg, I can see that x is the 0 component and y the 1 component. I hope bgr8 stays bgr8 and we don't flip things around here!
self.cv_image_stack.append(cv_image[:,:,0])
self.cv_image_stack.append(cv_image[:,:,1])
# self.cv_image_stack.extend([cv_image[:,:,0], cv_image[:,:,1]])
if len(self.cv_image_stack)>2*self.stack_depth: #keeps at most 2*self.stack_depth images in cv_image_stack, the 2 comes from the fact that we are using flow_x and flow_y
self.cv_image_stack.pop(0)
self.cv_image_stack.pop(0)
if self.stack_count%self.step == 0:
rospy.logdebug("reached execution part of callback!")
self.stack_count = 0 ## we don't keep a large number here.
scores = None
### i can maybe use a lambda to abstract this. is it faster than an if though?
if self.rgbOrFlow == 'rgb':
scores = self.net.predict_single_frame([cv_image,], 'fc-action', frame_size=(self.framesize_width, self.framesize_height))
elif self.rgbOrFlow == 'flow' and len(self.cv_image_stack)==10:
scores = self.net.predict_single_flow_stack(self.cv_image_stack, 'fc-action', frame_size=(self.framesize_width, self.framesize_height))
#print((scores))
if isinstance(scores, np.ndarray):
#this publishes the instant time version, aka, per frame
self.label_pub.pub([scores])
rospy.logdebug("published the label for instant time version!")
#this part publishes the frame_window version
self.frame_scores.append(scores)
if len(self.frame_scores)>self.classwindow:
self.frame_scores.pop(0)
self.label_fw_pub.pub(self.frame_scores)
rospy.logdebug("published the label for the frame window version!")
with self.lock:
if self.startedownvid:
self.ownvidscores.append(scores)
else:
rospy.logdebug_throttle(20,"waiting for start_vidscores call to start classifying ownvid")
self.stack_count = self.stack_count + 1
def forward_kinematics(self):
"""
Calculate current twist of the rover given current drive and corner motor velocities
Also approximate current turning radius.
Note that forward kinematics means solving an overconstrained system since the corner
motors may not be aligned perfectly and drive velocities might fight each other
"""
# calculate current turning radius according to each corner wheel's angle
# corner motor angles should be flipped since different coordinate axes in this node (positive z up)
theta_fl = -self.curr_positions['corner_left_front']
theta_fr = -self.curr_positions['corner_right_front']
theta_bl = -self.curr_positions['corner_left_back']
theta_br = -self.curr_positions['corner_right_back']
# sum wheel angles to find out which direction the rover is mostly turning in
if theta_fl + theta_fr + theta_bl + theta_br > 0: # turning left
r_front_closest = self.d1 + self.angle_to_turning_radius(theta_fl)
r_front_farthest = -self.d1 + self.angle_to_turning_radius(
theta_fr)
r_back_closest = -self.d1 - self.angle_to_turning_radius(theta_bl)
r_back_farthest = self.d1 - self.angle_to_turning_radius(theta_br)
else: # turning right
r_front_farthest = self.d1 + self.angle_to_turning_radius(theta_fl)
r_front_closest = -self.d1 + self.angle_to_turning_radius(theta_fr)
r_back_farthest = -self.d1 - self.angle_to_turning_radius(theta_bl)
r_back_closest = self.d1 - self.angle_to_turning_radius(theta_br)
# get a best estimate of the turning radius by taking the median value (avg sensitive to outliers)
approx_turning_radius = sum(
sorted([
r_front_farthest, r_front_closest, r_back_farthest,
r_back_closest
])[1:3]) / 2.0
if math.isnan(approx_turning_radius):
approx_turning_radius = self.max_radius
rospy.logdebug_throttle(
1, "Current approximate turning radius: {}".format(
round(approx_turning_radius, 2)))
self.curr_turning_radius = approx_turning_radius
# we know that the linear velocity in x direction is the instantaneous velocity of the middle virtual
# wheel which spins at the average speed of the two middle outer wheels.
drive_angular_velocity = (
self.curr_velocities['drive_left_middle'] +
self.curr_velocities['drive_right_middle']) / 2.
self.curr_twist.twist.linear.x = drive_angular_velocity * self.wheel_radius
# now calculate angular velocity from its relation with linear velocity and turning radius
self.curr_twist.twist.angular.z = self.curr_twist.twist.linear.x / self.curr_turning_radius
# covariance
self.curr_twist.covariance = 36 * [
0.0,
]
def getPrettyText(self, qtc_state):
qtc_data = qtc_state.split(',')
human_h = qtc_data[0]
robot_h = qtc_data[1]
human_v = qtc_data[2]
robot_v = qtc_data[3]
text_h = 'H:' + self.getPhrase(human_h, human_v)
text_r = 'R:' + self.getPhrase(robot_h, robot_v)
text = text_h + '\n' + text_r
rospy.logdebug_throttle(1, "Node [" + rospy.get_name() + "] " + text)
return text
def estimation(self, msg=None):
self.mutex.acquire()
try:
hpp = self.hpp()
q_current = hpp.robot.getCurrentConfig()
self._initialize_constraints(q_current)
# The optimization expects a configuration which already satisfies the constraints
projOk, q_projected, error = hpp.problem.applyConstraints(
q_current)
if projOk:
optOk, q_estimated, error = hpp.problem.optimize(q_projected)
if not optOk:
from numpy.linalg import norm
errNorm = norm(error)
if errNorm > 1e-2:
rospy.logwarn_throttle(
1, "Optimisation failed ? error norm: {0}".format(
errNorm))
rospy.logdebug_throttle(
1, "estimated == projected: {0}".format(
q_projected == q_estimated))
else:
rospy.loginfo_throttle(
1, "Optimisation failed ? error norm: {0}".format(
errNorm))
rospy.logdebug_throttle(1, "Error {0}".format(error))
valid, msg = hpp.robot.isConfigValid(q_estimated)
if not valid:
rospy.logwarn_throttle(
1, "Estimation in collision: {0}".format(msg))
self.publishers["estimation"]["semantic"].publish(q_estimated)
self.publish_state(hpp)
else:
hpp.robot.setCurrentConfig(q_current)
q_estimated = q_current
rospy.logwarn_throttle(
1, "Could not apply the constraints {0}".format(error))
except Exception as e:
rospy.logerr_throttle(1, str(e))
rospy.logerr_throttle(1, traceback.format_exc())
finally:
self.last_stamp_is_ready = False
self.mutex.release()
def cmd_cb(self, twist_msg, intuitive=False):
"""
Respond to an incoming Twist command in one of two ways depending on the mode (intuitive)
The Mathematically correct mode (intuitive=False) means that
* when the linear velocity is zero, an angular velocity does not cause the corner motors to move
(since simply steering the corners while standing still doesn't generate a twist)
* when driving backwards, steering behaves opposite as what you intuitively might expect
(this is to hold true to the commanded twist)
Use this topic with a controller that generated velocities based on targets. When you're
controlling the robot with a joystick or other manual input topic, consider using the
/cmd_vel_intuitive topic instead.
The Intuitive mode (intuitive=True) means that sending a positive angular velocity (moving joystick left)
will always make the corner wheels turn 'left' regardless of the linear velocity.
:param intuitive: determines the mode
"""
desired_turning_radius = self.twist_to_turning_radius(twist_msg, intuitive_mode=intuitive)
rospy.logdebug_throttle(1, "desired turning radius: {}".format(desired_turning_radius))
corner_cmd_msg = self.calculate_corner_positions(desired_turning_radius)
# if we're turning, calculate the max velocity the middle of the rover can go
max_vel = abs(desired_turning_radius) / (abs(desired_turning_radius) + self.d1) * self.max_vel
if math.isnan(max_vel): # turning radius infinite, going straight
max_vel = self.max_vel
velocity = min(max_vel, twist_msg.linear.x)
rospy.logdebug_throttle(1, "velocity drive cmd: {} m/s".format(velocity))
drive_cmd_msg = self.calculate_drive_velocities(velocity, desired_turning_radius)
rospy.logdebug_throttle(1, "drive cmd:\n{}".format(drive_cmd_msg))
rospy.logdebug_throttle(1, "corner cmd:\n{}".format(corner_cmd_msg))
if self.corner_cmd_threshold(corner_cmd_msg):
self.corner_cmd_pub.publish(corner_cmd_msg)
self.drive_cmd_pub.publish(drive_cmd_msg)
def publish(self):
if not self.base_waypoints or not self.pose:
return
index = self.get_next_waypoint_index()
waypoints = self.get_final_waypoints(index)
waypoints = self.adjust_velocity(waypoints, index)
final_waypoints_msg = Lane()
final_waypoints_msg.waypoints = waypoints
self.final_waypoints_pub.publish(final_waypoints_msg)
rospy.logdebug_throttle(
1, 'Position {}, Light {}'.format(index, self.light_wp - index))
def process_program_id(self, uint16_msg):
try:
if uint16_msg.data == 0:
rospy.logdebug_throttle(5, "Received script_id: 0 for reset")
self.script_id_zero_received = True
return
if not self.script_id_zero_received:
rospy.logdebug_throttle(
5,
"Avoiding script execution because script_id: 0 has not been received yet"
)
return
self.script_id_zero_received = False
self.execution_status_publisher.publish(
UInt8(self.ExecutionStatus.RESET))
time.sleep(self.sleep_time_in_seconds_between_status_messages)
self.execution_status_publisher.publish(
UInt8(self.ExecutionStatus.PROCESSING))
time.sleep(self.sleep_time_in_seconds_between_status_messages)
rospy.loginfo("process_program_id: [%d]", uint16_msg.data)
if uint16_msg.data > 0 and uint16_msg.data <= len(
self.scripts_configuration):
command = str(self.scripts_configuration[uint16_msg.data - 1])
command = command.replace("#", str(self.program_argument.data))
command = command.replace("$", self.scripts_directory + "/")
rospy.loginfo("command: [%s]", command)
time.sleep(self.sleep_time_in_seconds_before_running_script)
result_status = subprocess.call(command,
shell=True,
executable='/bin/bash')
rospy.loginfo("command return code: [%s]", result_status)
if result_status == 0:
self.execution_status_publisher.publish(
UInt8(self.ExecutionStatus.SUCCESS))
else:
self.execution_status_publisher.publish(
UInt8(self.ExecutionStatus.FAILED))
else:
rospy.logerr(
'[script_executor:process_program_id] Received program_id [%s] but it must be > 0 and <= size of scripts_configuration (current size: %s)',
str(uint16_msg.data), str(len(self.scripts_configuration)))
self.execution_status_publisher.publish(
UInt8(self.ExecutionStatus.FAILED))
except Exception as e:
rospy.logerr('[script_executor:process_program_id] Exception: %s',
str(e))
self.execution_status_publisher.publish(
UInt8(self.ExecutionStatus.FAILED))
def publish_debug_data(self, label, data):
"""
Publish debug data. Can be viewed using the dsd-visualization
This method is safe to call without wrapping it in a try-catch block although invalid values will
be wrapped in a `str()` call
:type label: str
:type data: dict or list or int or float or str or bool
"""
if type(data) not in (dict, list, int, float, str, bool):
rospy.logdebug_throttle(1, "The supplied debug data of type {} is not JSON serializable and will be wrapped in str()".format(type(data)))
data = str(data)
rospy.logdebug('{}: {}'.format(label, data))
self._debug_data[label] = data
def reports_callback(self, msg):
self.state = msg.state
self.controller_status = msg.status
rospy.logdebug_throttle(1,"Node [" + rospy.get_name() + "] \n- Vehicle State [" + self.status_dict['VehicleState'] + "]" +
"\n- Controller State [" + self.status_dict['ControllerState'] + "]" +
"\n- Control Status [" + self.status_dict['ControlStatus'] + "]" )
if (self.controller_status == ControllerReport.CONTROLLER_STATUS_WAIT) and (self.goalPS!=None):
rospy.logdebug("Node [" + rospy.get_name() + "] Goal to be send in some seconds ...")
rospy.sleep(5.1)
rospy.logdebug("Node [" + rospy.get_name() + "] Asking for a new one ......................")
self.goalPS.header.stamp = rospy.Time.now()
self.goal_topic_pub.publish(self.goalPS)
self.goalPS=None
rospy.logdebug("Node [" + rospy.get_name() + "] Done")
def check_and_submit(self, new_position, new_rotation):
""" Checks if position has changed significantly before submmitting a message """
current_translation = np.array([new_position.x, new_position.y, new_position.z])
if all(np.isclose(current_translation, self.last_translation, atol=1.e-4)):
return # no significant change, abort
self.last_translation = current_translation
_, _, yaw = euler_from_quaternion(
quaternion=(new_rotation.x, new_rotation.y, new_rotation.z, new_rotation.w))
msg = self.compose_modify_terrain_ellipse_message(new_position, degrees(yaw))
self.visual_pub.publish(msg)
rospy.logdebug_throttle(1, "modify_terrain_scoop message:\n" + str(msg))
def ir_array_filter(self, data):
if self.count < NUM_SAMPLE:
self.ir_array_sum = [
x + y for x, y in zip(self.ir_array_sum, data.ranges)
]
# print("count: " + str(self.count) + " ir_arr_sum: " + str(self.ir_array_sum))
self.count += 1
else:
self.ir_array_filtered[:] = [
x / NUM_SAMPLE for x in self.ir_array_sum
]
self.ir_front = self.ir_array_filtered[0:NUM_SS_FRONT]
self.ir_back = self.ir_array_filtered[NUM_SS_FRONT:NUM_SENSOR]
rospy.logdebug_throttle(
1, "ir_array_filtered: {}".format(
self._list(self.ir_array_filtered)))
self.count = 0
self.ir_array_sum = NUM_SENSOR * [0]