def publisher_node(self):
rospy.sleep(2)
twist = Twist()
init_t = rospy.get_time()
rate = rospy.Rate(10)
#Line-following Controller Initialization
desired = 300 # Line index value at which robot is centred
k_p = 0.0057
correction = 0
#Bayesian Localization Parameter Initialization
state_model = [0.05, 0.1, 0.85] # State model for u_k = +1
meas_model = {
0: {
0: 0.6,
1: 0.05,
2: 0.2,
3: 0.05
}, #green
1: {
0: 0.05,
1: 0.6,
2: 0.05,
3: 0.15
}, #orange
2: {
0: 0.2,
1: 0.05,
2: 0.6,
3: 0.05
}, #blue
3: {
0: 0.05,
1: 0.2,
2: 0.05,
3: 0.65
}, #yellow
4: {
0: 0.1,
1: 0.1,
2: 0.1,
3: 0.1
}
} #line
#Measurement_model represented as dictionary with keys as measurement z_k and value being a dictionary with key x_k and corresponding probabilities
pred = np.zeros(12) # Position prediction at each office location
update = np.zeros(
12, dtype=np.float
) # Update to state estimation probabilities based on prediction and measurement model
norm = np.zeros(
12
) # Normalization constant at each update to the state estimation
curr_state = [1 / float(len(pred))] * len(
pred
) # Current state estimation, starting with uniform probability distribution
# Reference RGB colour codes
ref = np.zeros((5, 3))
ref[0] = [133, 163, 1] #green
ref[1] = [222, 48, 1] #orange
ref[2] = [35, 60, 82] #blue
ref[3] = [171, 140, 2] #yellow
ref[4] = [120, 96, 34] #line
j = 0
while rospy.get_time() - init_t < (250):
actual = int(self.line_idx)
colour = None
rgb_error = np.abs(ref - self.measured_rgb)
rgb_sum = np.sum(rgb_error, axis=1)
min_ind = np.argmin(rgb_sum) # Colour estimation
if ((min_ind == 0) or (min_ind == 1) or (min_ind == 2)
or (min_ind == 3)): # If a colour is seen
correction = 0
colour = min_ind
elif (min_ind == 4
): # If no colour measured/seen (detected a line)
error = desired - actual
correction = k_p * error # Correction for P control
twist.angular.z = correction
twist.linear.x = 0.05
self.cmd_pub.publish(twist)
curr_office = np.argmax(curr_state) + 1 # Current state prediction
print(curr_state, (curr_office))
if correction == 0 and colour != None: # If a colour is seen
if curr_office == 4 or curr_office == 6 or curr_office == 8: # If we are at our chosen office location for delivery
init_2 = rospy.get_time()
rospy.sleep(3)
while rospy.get_time() - init_2 < (
(math.pi) / .32): #90 degree left rotation
twist.linear.x = 0
twist.angular.z = 0.2
self.cmd_pub.publish(twist)
rate.sleep()
init_3 = rospy.get_time()
rospy.sleep(1) # Pause
while rospy.get_time() - init_3 < (
(math.pi) / .48): #90 degree right rotation
twist.linear.x = 0
twist.angular.z = -0.2
self.cmd_pub.publish(twist)
rate.sleep()
# Resume straight-line motion
twist.linear.x = 0.05
twist.angular.z = 0
self.cmd_pub.publish(twist)
rospy.sleep(3)
else:
rospy.sleep(6.69) # Continue straight-line motion
for i in range(0, len(curr_state)): # Position prediction step
if (i == 0): # First point in map
step_fwd = curr_state[len(curr_state) -
1] * state_model[2]
if (i != 0):
step_fwd = curr_state[i -
1] * state_model[2] #loop around
if (i == len(curr_state) - 1):
step_bwd = curr_state[0] * state_model[0]
if (i != len(curr_state) - 1):
step_bwd = curr_state[i + 1] * state_model[0]
step_none = curr_state[i] * state_model[1]
pred[i] = (step_fwd + step_bwd + step_none)
for k in range(0, len(pred)): #update based on measurement
update[k] = pred[k] * meas_model[colour][color_map[k]]
norm[j] += update[k]
update = [float(x) / float(norm[j]) for x in update]
curr_state = update # This evolves at each colour measurement
j += 1 # Increment j every time you get a measurement
rate.sleep()
pass
def __init__(self):
rospy.init_node('turtlebot3_pointop_key', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
position = Point()
move_cmd = Twist()
r = rospy.Rate(10)
self.tf_listener = tf.TransformListener()
self.odom_frame = '/odom'
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
(position, rotation) = self.get_odom()
last_rotation = 0
linear_speed = 1
angular_speed = 1
(goal_x, goal_y, goal_z) = self.getkey()
if goal_z > 180 or goal_z < -180:
print("you input worng z range.")
self.shutdown()
goal_z = np.deg2rad(goal_z)
goal_distance = sqrt(
pow(goal_x - position.x, 2) + pow(goal_y - position.y, 2))
path_angle = atan2(goal_y, goal_x)
distance = goal_distance
while distance > 0.05:
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
path_angle = atan2(goal_y - y_start, goal_x - x_start)
if path_angle < -pi / 4 or path_angle > pi / 4:
if goal_y < 0 and y_start < goal_y:
path_angle = -2 * pi + path_angle
elif goal_y >= 0 and y_start > goal_y:
path_angle = 2 * pi + path_angle
if last_rotation > pi - 0.1 and rotation <= 0:
rotation = 2 * pi + rotation
elif last_rotation < -pi + 0.1 and rotation > 0:
rotation = -2 * pi + rotation
move_cmd.angular.z = angular_speed * path_angle - rotation
distance = sqrt(
pow((goal_x - x_start), 2) + pow((goal_y - y_start), 2))
move_cmd.linear.x = min(linear_speed * distance, 0.1)
if move_cmd.angular.z > 0:
move_cmd.angular.z = min(move_cmd.angular.z, 1.5)
else:
move_cmd.angular.z = max(move_cmd.angular.z, -1.5)
last_rotation = rotation
last_x = x_start
last_y = y_start
self.cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
while abs(rotation - goal_z) > 0.05:
(position, rotation) = self.get_odom()
if goal_z >= 0:
if rotation <= goal_z and rotation >= goal_z - pi:
move_cmd.linear.x = 0.00
move_cmd.angular.z = 0.5
else:
move_cmd.linear.x = 0.00
move_cmd.angular.z = -0.5
else:
if rotation <= goal_z + pi and rotation > goal_z:
move_cmd.linear.x = 0.00
move_cmd.angular.z = -0.5
else:
move_cmd.linear.x = 0.00
move_cmd.angular.z = 0.5
self.cmd_vel.publish(move_cmd)
r.sleep()
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
marker_wind = Marker_rviz("wind", (0, 0, 0), (0, 0, 0),
(awind + 0.01, 0.1, 0.1), 0)
marker_line = Marker_rviz("line", (0, 0, 0), (0, 0, 0), (.2, 0, 0), 4,
(0, 1, 1))
marker_thetabar = Marker_rviz("thetabar", (0, 0, 0), (0, 0, 0),
(3, 0.2, 0.2), 0, (0, 1, 1))
marker_zone = Marker_rviz("zone", (0, 0, -0.2), (0, 0, 0), (30, 30, 0.1),
3, (0, 0.5, 0))
while lat_lon_origin == [[], []] and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("[{}] Waiting GPS origin".format(node_name))
rospy.loginfo("[{}] Got GPS origin {}".format(node_name, lat_lon_origin))
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
marker_boat.publish()
marker_rudder.publish()
marker_sail.publish()
marker_wind.publish()
marker_line.publish()
marker_zone.publish()
marker_thetabar.publish()
rate.sleep()
br_boat = tf.TransformBroadcaster()
br_boat.sendTransform((x, y, 0.0),
quaternion_from_euler(roll, pitch, yaw),
rospy.Time.now(), "boat", "map")
br_wind = tf.TransformBroadcaster()
str(err_sum / ((openTime + gazebo_time) / 2)))
if simulating:
print("Sync Timestamp Simulating: ", timestamp, gazebo_time)
#if simulating and (timestamp>rosTime):
# rospy.loginfo( "Sync Timestamp: " , timestamp , rosTime)
#pauseOpenwsn = True
print "pauseOpenwsn: ", pauseOpenwsn
return pauseOpenwsn
def isRosSynced(self):
synced = not simulating
return synced
server.register_instance(MyFuncs())
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('mote_handler', anonymous=True)
rate = rospy.Rate(.5)
print "node initialized"
rospy.Subscriber("imu_datastream", Imu, imu_callback)
rospy.Subscriber("sim_status", Bool, sim_status_callback)
rospy.Subscriber("gazebo_time", Float32, gazebo_time_callback)
pause_pub = rospy.Publisher("gazebo_pause", Bool, queue_size=1)
# Run the server's main loop
server.serve_forever()
move.linear.x = -0.25
move.angular.z = 0
pub.publish(move)
# print('reverse')
def pause():
move.linear.x = 0
move.angular.z = 0
pub.publish(move)
# print('pause')
rospy.init_node('back_forth')
rate = rospy.Rate(50)
pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
move = Twist()
sequence = [forward, pause, reverse, pause]
duration = [3.0, 0.2, 3.0,
0.2] # use this so each step can have it's own duration
time_mark = rospy.get_time() # initialize
step = 0 # initialize
while not rospy.is_shutdown():
sequence[step]() # call the step
now = rospy.get_time()
if now - time_mark > duration[step]:
time_mark = now
step = (step + 1) % 4 # cycle through steps
rate.sleep()
skeleton.segments['Foot']['Ankle_R'].child = [['Foot','Intertarsal_R'],['Toes','Metatarsophalangeal1_R'],['Toes','Metatarsophalangeal2_R'],['Toes','Metatarsophalangeal3_R'],['Toes','Metatarsophalangeal4_R'],['Toes','Metatarsophalangeal5_R']]
skeleton.segments['Toes']['Metatarsophalangeal1_R'].child =[['Toes','Interphalangeal1_R']]
skeleton.segments['Toes']['Metatarsophalangeal2_R'].child =[['Toes','Interphalangeal2_R']]
skeleton.segments['Toes']['Metatarsophalangeal3_R'].child =[['Toes','Interphalangeal3_R']]
skeleton.segments['Toes']['Metatarsophalangeal4_R'].child =[['Toes','Interphalangeal4_R']]
skeleton.segments['Toes']['Metatarsophalangeal5_R'].child =[['Toes','Interphalangeal5_R']]
skeleton.addParents()
skeleton.segments['Trunk']['root'].addIMU(imu('/temp_4'))
skeleton.segments['Shoulder']['Sternoclavicular_L'].addIMU(imu('/l_shoulder'))
skeleton.segments['Shoulder']['Sternoclavicular_R'].addIMU(imu('/r_shoulder'))
skeleton.segments['Forearm']['Elbow_R'].addIMU(imu('/r_lower_arm'))
skeleton.segments['Arm']['Shoulder_R'].addIMU(imu('/r_upper_arm'))
skeleton.segments['Hand']['Wrist_R'].addIMU(imu('/r_hand'))
skeleton.segments['Arm']['Shoulder_L'].addIMU(imu('/l_upper_arm'))
skeleton.segments['Forearm']['Elbow_L'].addIMU(imu('/l_lower_arm'))
skeleton.segments['Hand']['Wrist_L'].addIMU(imu('/l_hand'))
skeleton.segments['Trunk']['Intervertebral3'].addIMU(imu('/chest'))
#skeleton.segments['Thigh']['Hip_R'].addIMU(imu('/temp_2'))
#skeleton.segments['Leg']['Knee_R'].addIMU(imu('/r_lower_leg'))
#skeleton.segments['Foot']['Ankle_R'].addIMU(imu('/r_foot'))
#skeleton.segments['Thigh']['Hip_L'].addIMU(imu('/l_upper_leg'))
#skeleton.segments['Leg']['Knee_L'].addIMU(imu('/l_lower_leg'))
#skeleton.segments['Foot']['Ankle_L'].addIMU(imu('/l_foot'))
while not rospy.is_shutdown():
r=rospy.Rate(60)
r.sleep()
skeleton.update()
def __init__(self):
freq = 200
rospy.init_node('grasp', anonymous=True)
self.init_broadcasts()
self.init_topics()
self.init_params()
rate = rospy.Rate(freq)
start = time.time()
print("Starting loop")
rot_mat = np.ones(4)
while not rospy.is_shutdown():
# Get End effector of robot
try:
trans_ee_real = self.listener.lookupTransform(
'world', 'iiwa_link_ee', rospy.Time(0))
if not self.ee_svr_logged:
rospy.loginfo("ee_real transform received")
self.ee_svr_logged = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
self.trans_ee_real = np.array(trans_ee_real[0])
# Get target in robot frame
try:
# common_time = self.listener.getLatestCommonTime(
# '/palm_link', '/target')
trans_world = self.listener.lookupTransform(
'mocap_world', 'world', rospy.Time(0))
if not self.trans_world_logged:
rospy.loginfo("world transform received")
self.trans_world_logged = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
try:
# common_time = self.listener.getLatestCommonTime(
# '/palm_link', '/target')
trans_target_raw = self.listener.lookupTransform(
'mocap_world', 'target_position', rospy.Time(0))
if not self.trans_target_logged:
rospy.loginfo("target transform received")
self.trans_target_logged = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
if not self.target_read:
trans_target = trans_target_raw
trans_target_raw_rotated = tf.transformations.quaternion_multiply(
trans_target_raw[1], trans_world[1])
rospy.loginfo("Target position and orientation locked")
self.target_read = True
# trans_target=trans_target_raw
# trans_target_raw_rotated=tf.transformations.quaternion_multiply(trans_target_raw[1],trans_world[1])
# print(tf.transformations.quaternion_matrix(trans_target[1]),np.append(np.array(trans_world[0]),1))
## Use the orientaion given by PCA
offset = np.dot(
tf.transformations.quaternion_matrix(trans_target[1]),
np.array([-0.03, -0.04, 0,
1]))[:-1] #-0.03,-0.018 worked ok 20191022
trans_world_rotated = np.dot(
tf.transformations.quaternion_matrix(
tf.transformations.quaternion_inverse(trans_world[1])),
np.append(np.array(trans_world[0]), 1))
trans_target_rotated = np.dot(
tf.transformations.quaternion_matrix(
tf.transformations.quaternion_inverse(trans_world[1])),
np.append(np.array(trans_target[0]) + offset, 1))
self.trans_target = trans_target_rotated[:
-1] - trans_world_rotated[:-1] + np.array(
[
0, 0, 0.19
]) + self.salad_offset
print(self.salad_offset)
## Use a fixed orientation 170 degrees
# transformations.quaternion_inverse(trans_world[1])),np.append(np.array(trans_world[0]),1))
# trans_world_rotated=np.dot(tf.transformations.quaternion_matrix(tf.transformations.quaternion_inverse(trans_world[1])),np.append(np.array(trans_world[0]),1))
# trans_target_rotated=np.dot(tf.transformations.quaternion_matrix(tf.transformations.quaternion_inverse(trans_world[1])),np.append(np.array(trans_target[0]),1))
# self.trans_target=trans_target_rotated[:-1]-trans_world_rotated[:-1]+np.array([-0.02,-0.005,0.19])
## Use a fixed orientation 180 degrees
# trans_world_rotated=np.dot(tf.transformations.quaternion_matrix(tf.transformations.quaternion_inverse(trans_world[1])),np.append(np.array(trans_world[0]),1))
# trans_target_rotated=np.dot(tf.transformations.quaternion_matrix(tf.transformations.quaternion_inverse(trans_world[1])),np.append(np.array(trans_target[0]),1))
# self.trans_target=trans_target_rotated[:-1]-trans_world_rotated[:-1]+np.array([0.00,0.015,0.18])
# trans_target_rotated=np.dot(tf.transformations.quaternion_matrix(tf.transformations.quaternion_inverse(trans_world[1])),np.append(np.array(trans_target[0]),1))
# trans_target_rotated=np.dot(tf.transformations.quaternion_matrix(tf.transformations.quaternion_inverse(trans_world[1])),np.append(np.array(trans_target[0])+np.array([-0.005,-0.02,0.19]),1))
# trans_target_rotated=np.dot(tf.transformations.quaternion_matrix(tf.transformations.quaternion_inverse(trans_world[1])),np.append(np.array(trans_target[0])+np.array([-0.005,-0.02,0.0]),1))
# self.trans_target=trans_target_rotated[:-1]-trans_world_rotated[:-1]+np.array([0,0,0.25])
# self.trans_target=trans_world[0]
self.attack_position = self.trans_target + np.array([0, 0, 0.2])
# self.attack_position=self.trans_target
if not self.target_position_initialized:
self.target_position = self.attack_position
self.target_position_initialized = True
if self.go_to_init_possition:
# self.custom_command.data[0:3] = [0, np.deg2rad(90), 0]
# self.custom_command.data[3:6] = [0, 0, 0.03]
# t_start = time.time()
# counter = 0
# while counter < 50:
# # self.custom_command.data[0:3] = [0, (counter+1)/50*np.deg2rad(90), 0]
# self.RobotCommandPub.publish(self.custom_command)
# self.GrabPub.publish(0)
# counter = counter+1
# rate.sleep()
self.go_to_init_possition = False
raw_input('Waiting to start movement')
self.go_to_init_possition2 = True
if self.go_to_init_possition2:
self.custom_command.data[0:3] = [0, np.deg2rad(160), 0]
self.custom_command.data[3:6] = [0, 0, 0.03]
t_start = time.time()
counter = 0
while counter < 50:
self.RobotCommandPub.publish(self.custom_command)
self.GrabPub.publish(0)
counter = counter + 1
rate.sleep()
self.go_to_init_possition2 = False
raw_input('Waiting to start movement')
# self.desired_orientation=self.quat_to_direction(trans_target[1])
self.desired_orientation = self.quat_to_direction(
trans_target_raw_rotated)
# self.desired_orientation= [0, np.deg2rad(180),0]
self.current_position = self.trans_ee_real
direction_to_target = (self.target_position -
self.current_position)
distance_to_target = np.linalg.norm(direction_to_target)
self.desired_velocity = self.robot_gain * (direction_to_target)
desired_velocity_norm = np.linalg.norm(self.desired_velocity)
if desired_velocity_norm > self.max_vel:
self.desired_velocity = self.desired_velocity / desired_velocity_norm * self.max_vel
self.custom_command.data[0:3] = self.desired_orientation
self.custom_command.data[3:6] = self.desired_velocity
self.RobotCommandPub.publish(self.custom_command)
# self.br_ee_target.sendTransform(trans_target_rotated[:-1]-trans_world_rotated[:-1]+np.array([-0.025,0.009,0.0]), [0,0,0,1], rospy.Time.now(
# ), 'target_position_rf', "world")
self.br_ee_target.sendTransform(
trans_target_rotated[:-1] - trans_world_rotated[:-1] +
np.array([0, 0, 0.19]), trans_target_raw_rotated,
rospy.Time.now(), 'target_position_rf', "world")
self.br_ee_target_dbg.sendTransform(self.attack_position,
[0, 0, 0, 1], rospy.Time.now(),
'attack_position_rf', "world")
rospy.loginfo_throttle(
1, "Max speed: " + str(np.linalg.norm(self.desired_velocity)))
# rospy.loginfo_throttle(1, "Target: "+str(self.target_position))
if distance_to_target < 0.05 and self.attack_reached == False:
self.target_position = self.trans_target + np.array(
[0, 0, 0.05])
self.robot_gain = 6
self.attack_reached = True
elif distance_to_target < 0.02 and self.attack_reached == True and self.target_reached_init == False:
self.target_position = self.trans_target
self.robot_gain = 6
self.target_reached_init = True
elif distance_to_target < 0.01 and self.target_reached == False and self.target_reached_init == True:
counter = 0
while counter < 50:
self.GrabPub.publish(1)
counter = counter + 1
rate.sleep()
rospy.loginfo_throttle(1, ['Target reached'])
self.target_reached = True
if self.grasped == 1:
self.max_vel = 0.3
self.target_position = self.trans_target + np.array(
[0, 0, 0.3])
rospy.loginfo_throttle(1, ['Picked up object'])
rospy.loginfo_throttle(
1, ['Dist to target ' + str(distance_to_target)])
rate.sleep()
def __init__(self):
# Give the node a name
rospy.init_node('out_and_back', anonymous=True)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
msg = Bool()
# Publisher the robot's is achieved
#self.goal_achieved_pub = rospy.Publisher('/goal_achieved', Bool, queue_size=1)
self.imu_subscriber = rospy.Subscriber('start_IMU', Bool,
self.callback)
# How fast will we update the robot's movement?
rate = 10
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.15 meters per second
self.linear_speed = 0.6
# Set the travel distance in meters
goal_distance = 1.0
# Set the rotation speed in radians per second
self.angular_speed = 0.5
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
self.distance_tolerance = rospy.get_param("~distance_tolerance", 0.25)
self.angular_tolerance = rospy.get_param("~angular_tolerance", 0.01)
# Give tf some time to fill its buffer
rospy.sleep(1)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
################### Wait for the Pos topic to become available ##########################
goal_topic = '/PDR_position'
self.position_subscriber = rospy.Subscriber(goal_topic,
Point,
self.set_cmd_vel,
queue_size=1)
rospy.loginfo("Subscribing to PDR position...")
# Wait for the topic to become available
rospy.wait_for_message(goal_topic, Point)
def __init__(self):
"""
Returns a pose controller.
:return: Pose controller
:rtype: PoseController
"""
# Params
self.available_controllers = ['simple', 'dual_quaternion']
self.event = None
self.current_pose = None
self.target_pose = None
# Object to compute transformations.
self.listener = transformation_utils.GeometryTransformer()
# Maximum duration to wait for a transform (in seconds).
self.wait_for_transform = rospy.get_param('~wait_for_transform', 0.1)
# The type of controller to use.
self.controller_type = rospy.get_param("~controller_type", 'simple')
assert self.controller_type in \
self.available_controllers, "{} is not an available controller. " \
"Available controllers are: {}".format(
self.controller_type, self.available_controllers)
# The twist will be described with respect to this frame.
self.reference_frame = rospy.get_param("~reference_frame", None)
assert self.reference_frame is not None, "A reference frame must be specified."
# Proportional gains for each of the six dimensions.
self.gains = rospy.get_param('~gains', [1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
# Velocity limits for each of the six dimensions (in m/s, rad/s).
self.limits = rospy.get_param('~limits', [1.0, 1.0, 1.0, 0.2, 0.2, 0.2])
# Whether to synchronize all velocities such that they reach the target
# at the same time.
self.sync = rospy.get_param('~sync', False)
# Linear tolerance (in meters).
self.linear_tolerance = rospy.get_param('~linear_tolerance', 0.01)
# Angular tolerance (in degrees).
self.angular_tolerance = rospy.get_param('~angular_tolerance', 3.0)
# Node cycle rate (in Hz).
self.loop_rate = rospy.Rate(rospy.get_param('~loop_rate', 100))
# Publishers
self.event_out = rospy.Publisher("~event_out", std_msgs.msg.String, queue_size=10)
self.twist_out = rospy.Publisher(
"~twist_out", geometry_msgs.msg.TwistStamped, queue_size=1, tcp_nodelay=True)
# Subscribers
rospy.Subscriber("~event_in", std_msgs.msg.String, self.event_in_cb)
rospy.Subscriber(
"~current_pose", geometry_msgs.msg.PoseStamped, self.current_pose_cb)
rospy.Subscriber("~target_pose", geometry_msgs.msg.PoseStamped, self.target_pose_cb)
def callback3(self, messeage):
self.data_z = messeage.data
def callback4(self, messeage):
self.data_h = messeage.data
#def callback5(self,messeage):
# self.data_depth = messeage.data
def callback6(self, messeage):
self.data_w = messeage.data
def callback7(self, message):
self.data_c = message.data
if __name__ == '__main__':
rospy.init_node('move_robot')
main = Main()
rate = rospy.Rate(60)
while not rospy.is_shutdown():
main.x_data = main.datainput.data_x
main.y_data = main.datainput.data_y
main.z_data = main.datainput.data_z
main.h_data = main.datainput.data_h
# main.depth_data = main.datainput.data_depth
main.w_data = main.datainput.data_w
main.flag = main.datainput.data_c
main.mainsystem()
rospy.sleep(0.1)
rate.sleep()
#!/usr/bin/env python
import roslib
# roslib.load_manifest('learning_tf')
import rospy
import tf
if __name__ == '__main__':
rospy.init_node('fixed_tf_broadcaster')
br = tf.TransformBroadcaster()
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
#global coordinate frame odom
br.sendTransform((0.0, 0.0, 0.0),
(0.0, 0.0, 0.0, 1.0),
rospy.Time.now(),
"n_1/odom",
"odom")
rate.sleep()
def main():
urdfname="/data/ros/yue_ws_201903/src/visionbased_polishing/urdf/ur5.urdf"
filename="/data/ros/yue_ws_201903/src/tcst_pkg/yaml/cam_300_industry_20200518.yaml"
"the robot arm parameters are set as follows:"
ace=50
vel=0.1
urt=0
"the adjustable parameters are set as follows:"
detat=0.05
z=0.25
ratet=5
netf_zero = [0.0,0.0,0.0]
xdot = 0.01
ydot = 0.01
fd=[0.0,0.0,0.0]
# the original parameter is shown as follows:
# lamdaf=[-0.001/2,-0.001/2,-0.001/2]#
lamdaf=[0.001/2,0.001/2,0.001/2]
p0=VisonControl(filename,0,urdfname,netf_zero)
ur_reader = Urposition()
ur_sub = rospy.Subscriber("/joint_states", JointState, ur_reader.callback)
structure_xnynan=StructurePointxnynanRead()
xn_sub = rospy.Subscriber("/cross_line_xsubn", Float64, structure_xnynan.structure_point_xn_callback)
yn_sub = rospy.Subscriber("/cross_line_ysubn", Float64, structure_xnynan.structure_point_yn_callback)
an_sub = rospy.Subscriber("/cross_line_asubn", Float64, structure_xnynan.structure_point_an_callback)
uv_get=UVRead()
uv_sub=rospy.Subscriber("/camera_uv/uvlist", uv,uv_get.callback)
xd_get=StructurePointXdydzdRead()
xd_sub = rospy.Subscriber("/structure_xd", Float64, xd_get.structure_point_xd_callback)
yd_sub = rospy.Subscriber("/structure_yd", Float64, xd_get.structure_point_yd_callback)
tool_get=UrToolVelocityRead()
tool_velocity_sub=rospy.Subscriber("/tool_velocity", TwistStamped, tool_get.Ur_tool_velocity_callback)
netf_reader = NetfData()
netf_sub = rospy.Subscriber("/robotiq_ft_wrench", WrenchStamped, netf_reader.callback)
x_error_pub = rospy.Publisher("/feature_x_error", Float64, queue_size=10)
y_error_pub = rospy.Publisher("/feature_y_error", Float64, queue_size=10)
z_error_pub = rospy.Publisher("/feature_z_error", Float64, queue_size=10)
z_impedance_error_pub = rospy.Publisher("/z_impedance_error_info", Float64, queue_size=10)
y_impedance_error_pub = rospy.Publisher("/y_impedance_error_info", Float64, queue_size=10)
x_impedance_error_pub = rospy.Publisher("/x_impedance_error_info", Float64, queue_size=10)
z_depth_pub = rospy.Publisher("/camera_depth", Float64, queue_size=10)
now_uv_pub = rospy.Publisher("/nowuv_info", uv, queue_size=10)
ur_pub = rospy.Publisher("/ur_driver/URScript", String, queue_size=10)
rate = rospy.Rate(ratet)
count=1
marker_zero=[]
# while count<30:
while not rospy.is_shutdown():
try:
sturucture_point_xn=structure_xnynan.sturucture_point_xn_buf
sturucture_point_yn=structure_xnynan.sturucture_point_yn_buf
sturucture_point_an=structure_xnynan.sturucture_point_an_buf
"the modification part 1"
# force_list = netf_reader.ave_netf_force_data
force_list = np.array([0.0,0.0,0.0])
if len(sturucture_point_xn)!=0 and len(sturucture_point_yn)!=0 and len(sturucture_point_an)!=0:
"the modification part 2"
# if len(xd_get.sturucture_point_xd_buf)!=0 and len(xd_get.sturucture_point_yd_buf)!=0:
"the modification part 3"
# xd=xd_get.sturucture_point_xd_buf[-1]
# yd=xd_get.sturucture_point_yd_buf[-1]
# zd=0.15
xd=305
yd=255
point1=np.array([xd,yd])
xd,yd=p0.change_uv_to_cartisian(point1)
zd = sturucture_point_an[-1]
xnynan=[sturucture_point_xn[-1],sturucture_point_yn[-1],sturucture_point_an[-1]]
print "the image features are:",xnynan
print "the desired feature is:", xd,yd,zd
print "the force sensor values are: ",force_list
if len(force_list) != 0 and len(tool_get.Ur_tool_velocity_buf)!=0:
"obtain real-time force, desired force and joint speed"
netf=[force_list[0],force_list[1],force_list[2]]
q_now = ur_reader.ave_ur_pose
joint_speed,vcc=p0.get_joint_speed(xnynan, xd, yd, zd, q_now, xdot, 0 * ydot, count, netf, fd, lamdaf)
print "joint speed is:", joint_speed
# print "joint_speed,vcc\n", joint_speed,vcc
if abs(joint_speed[0])<=100 and abs(joint_speed[1])<=100 and abs(joint_speed[2])<=100 and abs(joint_speed[3])<=100 and abs(joint_speed[4])<=100 and abs(joint_speed[5])<=100:
"""publish feature error"""
detas = p0.get_feature_error_xyz(xnynan, xd, yd, zd)
x_error_pub.publish(detas[0])
y_error_pub.publish(detas[1])
z_error_pub.publish(-1*detas[2])
"""publish impedance error"""
x_impedance_error = tool_get.Ur_tool_velocity_buf[-1][0] - vcc[0]
y_impedance_error = tool_get.Ur_tool_velocity_buf[-1][1] - vcc[1]
z_impedance_error=tool_get.Ur_tool_velocity_buf[-1][2]-vcc[2]
x_impedance_error_pub.publish(x_impedance_error)
y_impedance_error_pub.publish(y_impedance_error)
z_impedance_error_pub.publish(z_impedance_error)
"""publish movej joints value"""
print "the present joints angle are:",q_now
detaangular = p0.get_deta_joint_angular(detat, xnynan, xd, yd, zd, q_now, xdot, 0*ydot, count,netf,fd,lamdaf)
q_pub_now=p0.get_joint_angular(q_now,detaangular)
print "the deta joints angular are:",detaangular
print "the published joints angle are:",q_pub_now
ss = "movej([" + str(q_pub_now[0]) + "," + str(q_pub_now[1]) + "," + str(q_pub_now[2]) + "," + str(
q_pub_now[3]) + "," + str(q_pub_now[4]) + "," + str(q_pub_now[5]) + "]," + "a=" + str(ace) + "," + "v=" + str(
vel) + "," + "t=" + str(urt) + ")"
ur_pub.publish(ss)
count += 1
print "count--------",count
rate.sleep()
# else:
# print "the desired value is not obtained"
except:
continue
def manipulate(goalPose,Rspeed,verbose=False):
'''
This function return a trajectory between initial pose and goal pose (manipulation skil).
'''
kpis={'execution time':0,'energy':0}
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('Joints_publisher', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander('manipulator')
rate = rospy.Rate(1)
if verbose:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
print "============ Printing robot state"
print group.get_current_pose().pose
print "============"
group.clear_pose_targets()
group_variable_values = group.get_current_joint_values()
#initialise target position
pose_target = geometry_msgs.msg.Pose()
pose_target.position.x = goalPose[0]
pose_target.position.y = goalPose[1]
pose_target.position.z = goalPose[2]
pose_target.orientation.x =goalPose[3]
pose_target.orientation.y =goalPose[4]
pose_target.orientation.z =goalPose[5]
pose_target.orientation.w =goalPose[6]
group.set_pose_target(pose_target)
plan1 = group.plan()
# Visualisation in RVIZ
if verbose:
print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
goal_reached=False
startTime=time.clock()
while not rospy.is_shutdown() and not(goal_reached):
curPose=group.get_current_pose()
print type(group.get_current_pose())
curPose_np=(np.array([curPose.pose.position.x,curPose.pose.position.y,curPose.pose.position.z,curPose.pose.orientation.x,curPose.pose.orientation.y,curPose.pose.orientation.z,curPose.pose.orientation.w]))
print 'target: ',pose_targetac
goal_reached= np.all(np.isclose(curPose_np,goalPose,atol=1e-3))
print goal_reached
group.go(wait=True)
executionTime=time.clock()-startTime
kpis['execution time']=executionTime
print 'Skil execution: Done!\n'
print kpis
return kpis
def control():
# sp = np.load("xy_sin.npy")
state_sub = rospy.Subscriber("/mavros/state",State,state_cb,queue_size=10)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
acutator_control_pub = rospy.Publisher("/mavros/actuator_control",ActuatorControl,queue_size=10)
local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",PoseStamped,queue_size=10)
mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",PoseStamped,queue_size=10)
imu_sub = rospy.Subscriber("/mavros/imu/data",Imu,imu_cb, queue_size=10)
local_pos_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, lp_cb, queue_size=10)
local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity", TwistStamped, lv_cb, queue_size=10)
act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub", ActuatorControl,act_cb,queue_size=10)
rospy.init_node('control',anonymous=True)
rate = rospy.Rate(50.0)
# print("*"*80)
while not rospy.is_shutdown() and not current_state.connected:
rate.sleep()
rospy.loginfo(current_state.connected)
# print("#"*80)
pose = PoseStamped()
# pose.pose.position.x = 0
# pose.pose.position.y = 0
# pose.pose.position.z = 3
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.Time.now()
i = 0
act = ActuatorControl()
flag1 = False
flag2 = False
prev_imu_data = Imu()
prev_time = rospy.Time.now()
prev_omega_x = 0
prev_omega_y = 0
prev_omega_z = 0
prev_vx = 0
prev_vy = 0
prev_vz = 0
delta_t = 0.02
count = 0
theta = 0.0
x_step = 0.0
# rospy.loginfo("Outside")
while not rospy.is_shutdown():
if current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
offb_set_mode_response = set_mode_client(offb_set_mode)
if offb_set_mode_response.mode_sent:
rospy.loginfo("Offboard enabled")
flag1 = True
last_request = rospy.Time.now()
else:
if current_state.armed == False:
arm_cmd_response = arming_client(arm_cmd)
if arm_cmd_response.success :
rospy.loginfo("Vehicle armed")
flag2 = True
last_request = rospy.Time.now()
# rospy.loginfo("Inside")
curr_time = rospy.Time.now()
if flag1 and flag2:
x_f.append(float(local_position.pose.position.x))
y_f.append(float(local_position.pose.position.y))
z_f.append(float(local_position.pose.position.z))
vx_f.append(float(local_velocity.twist.linear.x))
vy_f.append(float(local_velocity.twist.linear.y))
vz_f.append(float(local_velocity.twist.linear.z))
# print(local_velocity.twist.linear.x)
orientation = [imu_data.orientation.w,imu_data.orientation.x,imu_data.orientation.y ,imu_data.orientation.z]
(roll,pitch, yaw) = quaternion_to_euler_angle(imu_data.orientation.w,imu_data.orientation.x,imu_data.orientation.y ,imu_data.orientation.z)
r_f.append(float(mt.radians(roll)))
p_f.append(float(mt.radians(pitch)))
yaw_f.append(float(mt.radians(yaw)))
sin_r_f.append(mt.sin(float(mt.radians(roll))))
sin_p_f.append(mt.sin(float(mt.radians(pitch))))
sin_y_f.append(mt.sin(float(mt.radians(yaw))))
cos_r_f.append(mt.cos(float(mt.radians(roll))))
cos_p_f.append(mt.cos(float(mt.radians(pitch))))
cos_y_f.append(mt.cos(float(mt.radians(yaw))))
rs_f.append(float(imu_data.angular_velocity.x))
ps_f.append(float(imu_data.angular_velocity.y))
ys_f.append(float(imu_data.angular_velocity.z))
ix.append(float(ixx))
iy.append(float(iyy))
iz.append(float(izz))
m.append(float(mass))
u0.append(act_controls.controls[0])
u1.append(act_controls.controls[1])
u2.append(act_controls.controls[2])
u3.append(act_controls.controls[3])
# pose.pose.position.x = theta
# pose.pose.position.y = 6.0*mt.sin(theta*PI/6)
# pose.pose.position.z = 3
pose.pose.position.x = 2.0*mt.cos(theta*PI/6) + 2.0*mt.sin(theta*PI/6) - 2
pose.pose.position.y = theta
pose.pose.position.z = 3
theta += 1.0/10
x_des.append(pose.pose.position.x)
y_des.append(pose.pose.position.y)
z_des.append(pose.pose.position.z)
sin_y_des.append(yaw_des)
cos_y_des.append(yaw_des)
w_mag.append(0)
w_x.append(0)
w_y.append(0)
w_z.append(0)
n_t = curr_time - prev_time
#delta_t = float(n_t.nsecs) * 1e-9
a_x.append((float(local_velocity.twist.linear.x) - prev_vx)/delta_t)
a_y.append((float(local_velocity.twist.linear.y) - prev_vy)/delta_t)
a_z.append((float(local_velocity.twist.linear.z) - prev_vz)/delta_t)
prev_vx = float(local_velocity.twist.linear.x)
prev_vy = float(local_velocity.twist.linear.y)
prev_vz = float(local_velocity.twist.linear.z)
aplha_x.append((float(imu_data.angular_velocity.x) - prev_omega_x)/delta_t)
aplha_y.append((float(imu_data.angular_velocity.y) - prev_omega_y)/delta_t)
aplha_z.append((float(imu_data.angular_velocity.z) - prev_omega_z)/delta_t)
ax_f.append(float(imu_data.linear_acceleration.x))
ay_f.append(float(imu_data.linear_acceleration.y))
az_f.append(float(imu_data.linear_acceleration.z))
prev_omega_x = float(imu_data.angular_velocity.x)
prev_omega_y = float(imu_data.angular_velocity.y)
prev_omega_z = float(imu_data.angular_velocity.z)
# theta += 1.0/100
count += 1
local_pos_pub.publish(pose)
if(count >= data_points):
break
prev_time = curr_time
rate.sleep()
nn1_xy_input_state = np.array([vx_f,vy_f,vz_f, rs_f,ps_f,ys_f, sin_r_f, sin_p_f,sin_y_f, cos_r_f,cos_p_f,cos_y_f, u3],ndmin=2).transpose()
nn1_xy_grd_truth = np.array([a_x,a_y,a_z],ndmin=2).transpose()
nn2_xy_input_state = np.array([vx_f,vy_f,vz_f, rs_f,ps_f,ys_f, sin_r_f, sin_p_f,sin_y_f, cos_r_f,cos_p_f,cos_y_f, u0, u1, u2],ndmin=2).transpose()
nn2_xy_grd_truth = np.array([aplha_x,aplha_y,aplha_z],ndmin=2).transpose()
print('nn1_xy_input_state :',nn1_xy_input_state.shape)
print('nn1_xy_grd_truth :',nn1_xy_grd_truth.shape)
print('nn2_xy_input_state :',nn2_xy_input_state.shape)
print('nn2_xy_grd_truth :',nn2_xy_grd_truth.shape)
np.save('nn1_cos_sin_x_y_input_state.npy',nn1_xy_input_state)
np.save('nn1_cos_sin_x_y_grd_truth.npy',nn1_xy_grd_truth)
np.save('nn2_cos_sin_x_y_input_state.npy',nn2_xy_input_state)
np.save('nn2_cos_sin_x_y_grd_truth.npy',nn2_xy_grd_truth)
s_cos_sin_x_y = np.array([x_f, y_f, z_f,vx_f,vy_f,vz_f,sin_r_f,sin_p_f,sin_y_f,cos_r_f,cos_p_f,cos_y_f,rs_f,ps_f,ys_f,r_f,p_f,yaw_f],ndmin=2).transpose()
u_cos_sin_x_y = np.array([u0,u1,u2,u3],ndmin=2).transpose()
a_cos_sin_x_y = np.array([ax_f,ay_f,az_f],ndmin=2).transpose()
print('s_cos_sin_x_y :',s_cos_sin_x_y.shape)
print('u_cos_sin_x_y :',u_cos_sin_x_y.shape)
print('a_cos_sin_x_y :',a_cos_sin_x_y.shape)
np.save('s_cos_sin_x_y.npy' ,s_cos_sin_x_y)
np.save('u_cos_sin_x_y.npy' ,u_cos_sin_x_y)
np.save('a_cos_sin_x_y.npy' ,a_cos_sin_x_y)
def __init__(self):
self.cv_bridge = CvBridge()
self.publisher = rospy.Publisher('/gesture_channel', Int32, queue_size=1)
self.img_subscriber = rospy.Subscriber("/webcam_image", Image, self.callback)
cv2.namedWindow("Gesture and Contour Window")
self.rate = rospy.Rate(1000)
def __init__(self,
dim=2,
udp_ip='127.0.0.1',
udp_recv_port=8026,
udp_send_port=6001,
buffer_size=8192 * 4):
RosProcessingComm.__init__(self,
udp_ip=udp_ip,
udp_recv_port=udp_recv_port,
udp_send_port=udp_send_port,
buffer_size=buffer_size)
if rospy.has_param('framerate'):
self.frame_rate = rospy.get_param('framerate')
else:
self.frame_rate = 60.0
self.period = rospy.Duration(1.0 / self.frame_rate)
self.running = True
self.runningCV = threading.Condition()
self.rate = rospy.Rate(self.frame_rate)
self.lock = threading.Lock()
rospy.Subscriber('joy', Joy, self.joyCB)
self.human_control_pub = None
self.human_robot_pose_pub = None
self.initializePublishers()
self.dim = dim
self.num_goals = 2
assert (self.num_goals > 0)
self.goal_positions = npa([[0] * self.dim] * self.num_goals, dtype='f')
if rospy.has_param('max_cart_vel'):
self._max_cart_vel = np.array(rospy.get_param('max_cart_vel'))
else:
self._max_cart_vel = 0.25 * np.ones(self.dim)
rospy.logwarn(
'No rosparam for max_cart_vel found...Defaulting to max linear velocity of 50 cm/s and max rotational velocity of 50 degrees/s'
)
if rospy.has_param('width'):
self.width = rospy.get_param('width')
else:
self.width = 1200
if rospy.has_param('height'):
self.height = rospy.get_param('height')
else:
self.height = 900
self.user_vel = CartVelCmd()
_dim = [MultiArrayDimension()]
_dim[0].label = 'cartesian_velocity'
_dim[0].size = 2
_dim[0].stride = 2
self.user_vel.velocity.layout.dim = _dim
self.user_vel.velocity.data = np.zeros(self.dim)
self.user_vel.header.stamp = rospy.Time.now()
self.user_vel.header.frame_id = 'human_control'
self.human_robot_pose = np.zeros(self.dim) #UNUSED for the time being.
self.human_robot_pose_msg = Point()
self.getRobotPosition()
self.human_goal_pose = Float32MultiArray()
self.human_goal_pose.data = [
list(x) for x in list(self.goal_positions)
]
self.human_goal_pose_pub.publish(self.human_goal_pose)
self.data = CartVelCmd()
self._msg_dim = [MultiArrayDimension()]
self._msg_dim[0].label = 'cartesian_velocity'
self._msg_dim[0].size = 2
self._msg_dim[0].stride = 2
self.data.velocity.layout.dim = _dim
self.data.velocity.data = np.zeros(self.dim)
self.data.header.stamp = rospy.Time.now()
self.data.header.frame_id = 'human_control'
rospy.Service("point_robot_human_control/set_human_goals", GoalPoses,
self.set_human_goals)
self.send_thread = threading.Thread(target=self._publish_command,
args=(self.period, ))
self.send_thread.start()
rospy.loginfo("END OF CONSTRUCTOR - point_robot_human_control_node")
def process_trajectory(self, traj):
num_points = len(traj.points)
# make sure the joints in the goal match the joints of the controller
if set(self.joint_names) != set(traj.joint_names):
res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.INVALID_JOINTS)
msg = 'Incoming trajectory joints do not match the joints of the controller'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# make sure trajectory is not empty
if num_points == 0:
msg = 'Incoming trajectory is empty'
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
return
# correlate the joints we're commanding to the joints in the message
# map from an index of joint in the controller to an index in the trajectory
lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
durations = [0.0] * num_points
# find out the duration of each segment in the trajectory
durations[0] = traj.points[0].time_from_start.to_sec()
for i in range(1, num_points):
durations[i] = (traj.points[i].time_from_start - traj.points[i - 1].time_from_start).to_sec()
if not traj.points[0].positions:
res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.INVALID_GOAL)
msg = 'First point of trajectory has no positions'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
trajectory = []
time = rospy.Time.now() + rospy.Duration(0.01)
for i in range(num_points):
seg = Segment(self.num_joints)
if traj.header.stamp == rospy.Time(0.0):
seg.start_time = (time + traj.points[i].time_from_start).to_sec() - durations[i]
else:
seg.start_time = (traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i]
seg.duration = durations[i]
# Checks that the incoming segment has the right number of elements.
if traj.points[i].velocities and len(traj.points[i].velocities) != self.num_joints:
res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.INVALID_GOAL)
msg = 'Command point %d has %d elements for the velocities' % (i, len(traj.points[i].velocities))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
if len(traj.points[i].positions) != self.num_joints:
res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.INVALID_GOAL)
msg = 'Command point %d has %d elements for the positions' % (i, len(traj.points[i].positions))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
for j in range(self.num_joints):
if traj.points[i].velocities:
seg.velocities[j] = traj.points[i].velocities[lookup[j]]
if traj.points[i].positions:
seg.positions[j] = traj.points[i].positions[lookup[j]]
trajectory.append(seg)
rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec())
rate = rospy.Rate(self.update_rate)
while traj.header.stamp > time:
time = rospy.Time.now()
rate.sleep()
end_time = traj.header.stamp + rospy.Duration(sum(durations))
seg_end_times = [rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory))]
rospy.loginfo('Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), end_time.to_sec(), sum(durations))
self.trajectory = trajectory
traj_start_time = rospy.Time.now()
for seg in range(len(trajectory)):
rospy.logdebug('current segment is %d time left %f cur time %f' % (seg, durations[seg] - (time.to_sec() - trajectory[seg].start_time), time.to_sec()))
rospy.logdebug('goal positions are: %s' % str(trajectory[seg].positions))
# first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
if durations[seg] == 0:
rospy.logdebug('skipping segment %d with duration of 0 seconds' % seg)
continue
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
j = self.joint_names.index(joint)
start_position = self.joint_states[joint].current_pos
if seg != 0: start_position = trajectory[seg - 1].positions[j]
desired_position = trajectory[seg].positions[j]
desired_velocity = max(self.min_velocity, abs(desired_position - start_position) / durations[seg])
self.msg.desired.positions[j] = desired_position
self.msg.desired.velocities[j] = desired_velocity
# probably need a more elegant way of figuring out if we are dealing with a dual controller
if hasattr(self.joint_to_controller[joint], "master_id"):
master_id = self.joint_to_controller[joint].master_id
slave_id = self.joint_to_controller[joint].slave_id
master_pos, slave_pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
vals.append((master_id, master_pos, spd))
vals.append((slave_id, slave_pos, spd))
else:
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
vals.append((motor_id, pos, spd))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position_and_speed(vals)
while time < seg_end_times[seg]:
# check if new trajectory was received, if so abort current trajectory execution
# by setting the goal to the current position
if self.action_server.is_preempt_requested():
msg = 'New trajectory received. Aborting old trajectory.'
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
cur_pos = self.joint_states[joint].current_pos
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(cur_pos)
vals.append((motor_id, pos))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position(vals)
self.action_server.set_preempted(text=msg)
rospy.logwarn(msg)
return
rate.sleep()
time = rospy.Time.now()
# Verifies trajectory constraints
for j, joint in enumerate(self.joint_names):
if self.trajectory_constraints[j] > 0 and self.msg.error.positions[jjoint_trajectory_action_node] > self.trajectory_constraints[j]:
res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED)
msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
(joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# let motors roll for specified amount of time
rospy.sleep(self.goal_time_constraint)
for i, j in enumerate(self.joint_names):
rospy.logdebug('desired pos was %f, actual pos is %f, error is %f' % (trajectory[-1].positions[i], self.joint_states[j].current_pos, self.joint_states[j].current_pos - trajectory[-1].positions[i]))
# Checks that we have ended inside the goal constraints
for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.msg.error.positions, self.goal_constraints):
if pos_constraint > 0 and abs(pos_error) > pos_constraint:
res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED)
msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
(joint, pos_error, pos_constraint)
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
break
else:
res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.SUCCESSFUL)
msg = 'Trajectory execution successfully completed'
rospy.loginfo(msg)
self.action_server.set_succeeded(result=res, text=msg)
goal.pose.position.y = 0
goal.pose.position.z = 0.1
goal.pose.orientation.x = 0
goal.pose.orientation.y = 0.5*math.sqrt(2)
goal.pose.orientation.z = 0
goal.pose.orientation.w = 0.5*math.sqrt(2)
#print "Goal:\r\n %s" %goal
self.tf.waitForTransform(goal.header.frame_id, 'torso_lift_link', rospy.Time.now(), rospy.Duration(3.0))
appr = self.tf.transformPose('torso_lift_link', goal)
# print "Appr: \r\n %s" %appr
self.wt_log_out.publish(data="Normal Approach with right hand: Trying to move WITH motion planning")
#self.move_arm_out.publish(appr)
self.reactive_grasp(appr)
def linear_move(self, msg):
print "Linear Move: Right Arm: %s m Step" %msg.data
self.tf.waitForTransform(self.currpose.header.frame_id, 'r_wrist_roll_link', self.currpose.header.stamp, rospy.Duration(3.0))
newpose = self.tf.transformPose('r_wrist_roll_link', self.currpose)
newpose.pose.position.x += msg.data
step_goal = self.tf.transformPose(self.currpose.header.frame_id, newpose)
self.goal_out.publish(step_goal)
if __name__ == '__main__':
NA = NormalApproach()
r = rospy.Rate(10)
while not rospy.is_shutdown():
NA.tfb.sendTransform((NA.px,NA.py,NA.pz),(NA.qx,NA.qy,NA.qz,NA.qw), rospy.Time.now(), "pixel_3d_frame", NA.frame)
r.sleep()
def __init__(self):
# Give the node a name
rospy.init_node('out_and_back', anonymous=False)
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(self.shutdown)
# Publisher to control the robot's speed
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
# How fast will we update the robot's movement?
rate = 20
# Set the equivalent ROS rate variable
r = rospy.Rate(rate)
# Set the forward linear speed to 0.15 meters per second
linear_speed = 0.05
# Set the travel distance in meters
goal_distance = 1.0
# Set the rotation speed in radians per second
angular_speed = 0.15
# Set the angular tolerance in degrees converted to radians
angular_tolerance = radians(1.0)
# Set the rotation angle to Pi radians (180 degrees)
goal_angle = pi
# Initialize the tf listener
self.tf_listener = tf.TransformListener()
# Give tf some time to fill its buffer
rospy.sleep(2)
# Set the odom frame
self.odom_frame = '/odom'
# Find out if the robot uses /base_link or /base_footprint
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_footprint', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame,
'/base_link', rospy.Time(),
rospy.Duration(1.0))
self.base_frame = '/base_link'
except (tf.Exception, tf.ConnectivityException,
tf.LookupException):
rospy.loginfo(
"Cannot find transform between /odom and /base_link or /base_footprint"
)
rospy.signal_shutdown("tf Exception")
# Initialize the position variable as a Point type
position = Point()
# Loop once for each leg of the trip
for i in range(2):
# Initialize the movement command
move_cmd = Twist()
# Set the movement command to forward motion
move_cmd.linear.x = linear_speed
# Get the starting position values
(position, rotation) = self.get_odom()
x_start = position.x
y_start = position.y
# Keep track of the distance traveled
distance = 0
# Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current position
(position, rotation) = self.get_odom()
# Compute the Euclidean distance from the start
distance = sqrt(
pow((position.x - x_start), 2) +
pow((position.y - y_start), 2))
# Stop the robot before the rotation
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Set the movement command to a rotation
move_cmd.angular.z = angular_speed
# Track the last angle measured
last_angle = rotation
# Track how far we have turned
turn_angle = 0
while abs(turn_angle + angular_tolerance) < abs(
goal_angle) and not rospy.is_shutdown():
# Publish the Twist message and sleep 1 cycle
self.cmd_vel.publish(move_cmd)
r.sleep()
# Get the current rotation
(position, rotation) = self.get_odom()
# Compute the amount of rotation since the last loop
delta_angle = normalize_angle(rotation - last_angle)
# Add to the running total
turn_angle += delta_angle
last_angle = rotation
# Stop the robot before the next leg
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
# Stop the robot for good
self.cmd_vel.publish(Twist())
else:
rospy.loginfo("axis_map config set to " + str(axis_map))
if not(write_to_dev(ser, AXIS_MAP_SIGN, 1, axis_map_sign)):
rospy.logerr("Unable to set IMU axis signs.")
else:
rospy.loginfo("axis_map_sign set to " + str(axis_map_sign))
if not(write_to_dev(ser, OPER_MODE, 1, operation_mode)):
rospy.logerr("Unable to set IMU operation mode into operation mode.")
rospy.loginfo("Bosch BNO055 IMU configuration complete.")
rospy.loginfo("Imu frequency " + str(frequency))
rate = rospy.Rate(frequency)
# Factors for unit conversions
q_fact = 16384.0
acc_fact = 100.0
mag_fact = 16.0
gyr_fact = 900.0
seq = 0
while not rospy.is_shutdown():
buf = read_from_dev(ser, ACCEL_DATA, 45)
if buf != 0:
# Publish raw data
imu_raw.header.stamp = rospy.Time.now()
imu_raw.header.frame_id = frame_id
imu_raw.header.seq = seq
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
rospy.init_node("move_in_circle_node")
# create a publisher
cmd_pub = rospy.Publisher("/pioneer2dx/cmd_vel",Twist,queue_size=10)
rate = rospy.Rate(10)
cmd = Twist()
def publish_vel():
cmd.linear.x = 0.5 #0.0
cmd.angular.z = -0.25
while not rospy.is_shutdown():
cmd_pub.publish(cmd)
rate.sleep()
def stop():
cmd.linear.x = 0.0
cmd.angular.z = 0.0
cmd_pub.publish(cmd)
if __name__ == "__main__":
try:
rospy.on_shutdown(stop)
publish_vel()
except rospy.ROSInterruptException:
pass
reset_sim = rospy.ServiceProxy('/gazebo/reset_simulation', Empty)
set_model_srv = rospy.ServiceProxy('/gazebo/set_model_configuration',
SetModelConfiguration)
jointCmd = JointTrajectory()
point = JointTrajectoryPoint()
jointCmd.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
point.time_from_start = rospy.Duration.from_sec(1)
for i in range(0, 6):
jointCmd.joint_names.append('j2n6s300_joint_' + str(i + 1))
point.positions.append(joint_positions[i])
point.velocities.append(0)
point.accelerations.append(0)
point.effort.append(0)
jointCmd.points.append(point)
rate = rospy.Rate(100)
count = 0
topic_name = '/j2n6s300/effort_finger_trajectory_controller/command'
pubf = rospy.Publisher(topic_name, JointTrajectory, queue_size=1)
jointCmd_finger = JointTrajectory()
point = JointTrajectoryPoint()
jointCmd_finger.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.0)
point.time_from_start = rospy.Duration.from_sec(1)
for i in range(0, 3):
jointCmd_finger.joint_names.append('j2n6s300_joint_finger_' + str(i + 1))
point.positions.append(joint_positions[i + 6])
point.velocities.append(0)
point.accelerations.append(0)
point.effort.append(0)
jointCmd_finger.points.append(point)
elif modded_idx == 1:
return 'WizzyB'
elif modded_idx == 2:
return 'WizzyC'
else:
return 'WizzyClear'
if __name__ == "__main__":
rospy.init_node('hmi_tester')
chair_state_pub = rospy.Publisher('/chair_state', ChairState, queue_size=50)
time.sleep(1) # For letting the publisher subscribe in the server
chair_state = ChairState()
rate = rospy.Rate(0.2)
count = -2
while not rospy.is_shutdown():
chair_state.ttc_msg.ttc_azimuth = count * 1.047
chair_state.state.data = index_to_mode(count)
chair_state_pub.publish(chair_state)
print(chair_state.ttc_msg.ttc_azimuth, chair_state.state.data)
count += 1
if count == 4:
count = -2
rate.sleep()
# Create publisher to publish particle states
particles_pub = rospy.Publisher('/uwb/pf/particles',
PoseArray,
queue_size=10)
# Create publish to publish expected pose
pose_pub = rospy.Publisher('/uwb/pf/pose', PoseStamped, queue_size=10)
# Transform broadcaster to broadcast localization info
transform_broadcaster = tf.TransformBroadcaster()
# Transform listener is used to listen to the transform from odom --> base_link frame
transform_listener = tf.TransformListener()
rospy.loginfo("Beginning Particle Filtering")
# Publish at 20 Hz
rate = rospy.Rate(UPDATE_RATE) # 20hz
seq = 0
#Update initial estimate of pose
estimated_pose = pf.get_state()
while not rospy.is_shutdown():
#start = time.clock()
pf.update(
np.ma.masked_array(sensor_measurements,
mask=sensor_measurements_mask))
print(sensor_measurements_mask)
sensor_measurements_mask = np.ones(shape=(num_sensors))
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
import roslib; roslib.load_manifest('kobuki_testsuite')
import rospy
from kobuki_msgs.msg import Sound
sounds = [Sound.ON, Sound.OFF, Sound.RECHARGE, Sound.BUTTON, Sound.ERROR, Sound.CLEANINGSTART, Sound.CLEANINGEND]
texts = ["On", "Off", "Recharge", "Button", "Error", "CleaningStart", "CleaningEnd"]
rospy.init_node("test_sounds")
pub = rospy.Publisher('/mobile_base/commands/sound', Sound)
rate = rospy.Rate(0.5)
# Added below two line of code
# to wait until at least one subscriber is present or connected.
# Because rospy.Publisher will skip(or eat) first certain messages, if you publish just after rospy.init()
# Without this patch, first message - Sound.ON will never be published.
# I think this is a bug of rospy
# Younghun Ju
while not pub.get_num_connections():
rate.sleep()
msg = Sound()
while not rospy.is_shutdown():
for sound, text in zip(sounds, texts):
msg.value = sound
print text
def __init__(self, mode, scale_x, scale_z, rate):
self._mode = mode
self._scale_x = scale_x
self._scale_z = scale_z
self._timer = Timer()
self._rate = rospy.Rate(rate)
self._enable_auto_control = False
self.current_control = None
self.trajectory_index = None
self.bridge = CvBridge()
# callback data store
self.image = None
self.left_img = None
self.right_img = None
self.depth_img = None
self.me1_left = None
self.me1_right = None
self.me1_depth = None
self.me2_left = None
self.me2_right = None
self.me2_depth = None
self.me3_left = None
self.me3_right = None
self.me3_depth = None
self.intention = None
self.imu = None
self.imu1 = None
self.imu2 = None
self.imu3 = None
self.odom = None
self.speed = None
self.labeled_control = None
self.key = None
self.training = False
self.scan = None
self.manual_intention = 'forward'
self.is_manual_intention = False
# subscribe ros messages
rospy.Subscriber('/mynteye/left/image_raw/compressed',
CompressedImage,
self.cb_left_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye/right/image_raw/compressed',
CompressedImage,
self.cb_right_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye/depth/image_raw/compressed',
CompressedImage,
self.cb_depth_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_1/left/image_raw/compressed',
CompressedImage,
self.cb_me1_left_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_1/right/image_raw/compressed',
CompressedImage,
self.cb_me1_right_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_1/depth/image_raw/compressed',
CompressedImage,
self.cb_me1_depth_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_2/left/image_raw/compressed',
CompressedImage,
self.cb_me2_left_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_2/right/image_raw/compressed',
CompressedImage,
self.cb_me2_right_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_2/depth/image_raw/compressed',
CompressedImage,
self.cb_me2_depth_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_3/left/image_raw/compressed',
CompressedImage,
self.cb_me3_left_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_3/right/image_raw/compressed',
CompressedImage,
self.cb_me3_right_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/mynteye_3/depth/image_raw/compressed',
CompressedImage,
self.cb_me3_depth_img,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/scan',
LaserScan,
self.cb_scan,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/imu',
Imu,
self.cb_imu,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/imu1',
Imu,
self.cb_imu1,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/imu2',
Imu,
self.cb_imu2,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/imu3',
Imu,
self.cb_imu3,
queue_size=1,
buff_size=2**10)
if mode == 'DLM':
rospy.Subscriber('/test_intention',
String,
self.cb_dlm_intention,
queue_size=1)
else:
rospy.Subscriber('/intention_lpe',
Image,
self.cb_lpe_intention,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/speed', Float32, self.cb_speed, queue_size=1)
rospy.Subscriber('/odometry/filtered',
Odometry,
self.cb_odom,
queue_size=1,
buff_size=2**10)
rospy.Subscriber('/joy', Joy, self.cb_joy)
# publish control
self.control_pub = rospy.Publisher('/RosAria/cmd_vel',
Twist,
queue_size=1)
# publishing as training data
self.pub_intention = rospy.Publisher('/test_intention',
String,
queue_size=1)
self.pub_trajectory_index = rospy.Publisher('/train/trajectory_index',
String,
queue_size=1)
self.pub_teleop_vel = rospy.Publisher('/train/cmd_vel',
Twist,
queue_size=1)
self.pub_left_img = rospy.Publisher(
'/train/mynteye/left_img/compressed',
CompressedImage,
queue_size=1)
self.pub_right_img = rospy.Publisher(
'/train/mynteye/right_img/compressed',
CompressedImage,
queue_size=1)
self.pub_depth_img = rospy.Publisher(
'/train/mynteye/depth_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me1_left_img = rospy.Publisher(
'/train/mynteye_1/left_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me1_right_img = rospy.Publisher(
'/train/mynteye_1/right_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me1_depth_img = rospy.Publisher(
'/train/mynteye_1/depth_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me2_left_img = rospy.Publisher(
'/train/mynteye_2/left_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me2_right_img = rospy.Publisher(
'/train/mynteye_2/right_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me2_depth_img = rospy.Publisher(
'/train/mynteye_2/depth_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me3_left_img = rospy.Publisher(
'/train/mynteye_3/left_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me3_right_img = rospy.Publisher(
'/train/mynteye_3/right_img/compressed',
CompressedImage,
queue_size=1)
self.pub_me3_depth_img = rospy.Publisher(
'/train/mynteye_3/depth_img/compressed',
CompressedImage,
queue_size=1)
self.pub_intention = rospy.Publisher('/train/intention',
String,
queue_size=1)
self.pub_imu = rospy.Publisher('/train/imu', Imu, queue_size=1)
self.pub_imu1 = rospy.Publisher('/train/imu1', Imu, queue_size=1)
self.pub_imu2 = rospy.Publisher('/train/imu2', Imu, queue_size=1)
self.pub_imu3 = rospy.Publisher('/train/imu3', Imu, queue_size=1)
self.pub_odom = rospy.Publisher('/train/odometry/filtered',
Odometry,
queue_size=1)
self.pub_scan = rospy.Publisher('/train/scan', LaserScan, queue_size=1)
if __name__ == '__main__':
rospy.init_node('find_objects')
object_pub = rospy.Publisher('object_cloud', PointCloud2)
table_pub = rospy.Publisher('table', Table)
rate = rospy.get_param('~detect_rate', default=0.5)
br = TransformBroadcaster()
detect_srv = rospy.ServiceProxy('/tabletop_segmentation',
TabletopSegmentation)
detect_srv.wait_for_service()
Thread(target=rospy.Timer(rospy.Duration(0.05), broadcast_table_frame).run)
#clients = dict(
# x = dynamic_reconfigure.client.Client('xfilter', timeout=float('inf')),
# y = dynamic_reconfigure.client.Client('yfilter', timeout=float('inf')),
# z = dynamic_reconfigure.client.Client('zfilter', timeout=float('inf'))
#)
r = rospy.Rate(rate)
while not rospy.is_shutdown():
try:
object_cloud, table = detect(detect_srv)
if table is not None:
with tf_lock:
if not table_pose or np.linalg.norm(
pt2np(table_pose.pose.position) -
pt2np(table.pose.pose.position)) > 0.1:
table_pose = table.pose
table_pub.publish(table)
if object_cloud is not None:
#set_table_filter_limits(table, clients)
object_pub.publish(object_cloud)
r.sleep()
def acquire_images(self):
"""
This function acquires and saves 10 images from a device.
Please see Acquisition example for more in-depth comments on acquiring images.
:param cam: Camera to acquire images from.
:type cam: CameraPtr
:return: True if successful, False otherwise.
:rtype: bool
"""
#rospy.loginfo("***** BLACKFLY: IMAGE ACQUISITION ***\n")
try:
result = True
# Set acquisition mode to continuous
if self.cam.AcquisitionMode.GetAccessMode() != PySpin.RW:
rospy.loginfo("***** BLACKFLY: Unable to set acquisition mode to continuous. Aborting...")
return False
self.cam.AcquisitionMode.SetValue(PySpin.AcquisitionMode_Continuous)
#rospy.loginfo("***** BLACKFLY: Acquisition mode set to continuous...")
# Begin acquiring images
self.cam.BeginAcquisition()
rospy.loginfo("***** BLACKFLY: Acquiring images...")
# Get device serial number for filename
if self.cam.TLDevice.DeviceSerialNumber.GetAccessMode() == PySpin.RO:
self.ser_num = self.cam.TLDevice.DeviceSerialNumber.GetValue()
#rospy.loginfo("***** BLACKFLY: Device serial number retrieved as %s..." % self.ser_num)
# Retrieve, convert, and save images
bridge = CvBridge()
last_time = rospy.get_time()
n = 0
saved_count = 0
r = rospy.Rate(50) # 30hz
while not rospy.is_shutdown():
try:
#print("Time now1: %f" % (rospy.get_time() - last_time)) # 0.000086
# Retrieve the next image from the trigger
# Does nothing if Hardware trigger except print line.
result &= self.grab_next_image_by_trigger()
#print("Time now2: %f" % (rospy.get_time() - last_time)) # 0.000107
# Retrieve next received image
image_result = self.cam.GetNextImage(500) # timeout in milliseconds
#print("Time now3: %f" % (rospy.get_time() - last_time)) # 0.029838
# Ensure image completion
if image_result.IsIncomplete():
rospy.loginfo_throttle(5, "***** BLACKFLY: Image incomplete with image status %d ..." % image_result.GetImageStatus())
else:
n += 1
# Convert image to mono 8
image_converted = image_result.Convert(PySpin.PixelFormat_BGR8, PySpin.HQ_LINEAR)
image_data = image_converted.GetNDArray()
#print("Time now4: %f" % (rospy.get_time() - last_time)) # 0.045697
#Before taking a picture, grab timestamp to record to filename, and CSV
#self.tNow = rospy.get_time() # current date and time
#self.datetimeData = datetime.datetime.utcfromtimestamp(self.tNow).strftime('%Y%m%d_%H%M%S_%f')
self.datetimeData = self.trigtime
if (self.is_recording):
saved_count += 1
self.save_img(image_data, self.datetimeData)
#cv2.imshow("frame",image_data)
#cv2.waitKey(1)
try:
#cv2.putText(image_data,self.datetimeData[:-3],
cv2.putText(image_data,self.datetimeData,
(30,1280),
cv2.FONT_HERSHEY_SIMPLEX,
2,
(251,251,251),
3)
self.image_pub.publish(bridge.cv2_to_imgmsg(image_data, "bgr8"))
except CvBridgeError as e:
rospy.loginfo("***** BLACKFLY: %s" %e)
#print("Time now5: %f\n--------------------\n" % (rospy.get_time() - last_time)) # 0.049854
# Release image
image_result.Release()
rospy.loginfo_throttle(5," ***** BLACKFLY: Grabbed Image %d, and saved %d" % (n, saved_count))
except PySpin.SpinnakerException as ex:
rospy.loginfo_throttle(5, "***** BLACKFLY Error3: %s" % ex)
rospy.loginfo("\n***** BLACKFLY has stopeed. EXITING NOW. *************************************************\n")
exit()
last_time = rospy.get_time()
r.sleep()
# End acquisition
self.cam.EndAcquisition()
except PySpin.SpinnakerException as ex:
rospy.loginfo("***** BLACKFLY Error4: %s" % ex)
exit()
return False
return result
print((x, y))
# new Twist object
move = Twist()
# turn left or right or go straight
if x < 350:
move.linear.x = TURN_LEFT_SPD
move.angular.z = ANGULAR_VEL
pub.publish(move)
elif x >= 350 and x <= 450:
move.linear.x = STRAIGHT_SPD
move.angular.z = 0
pub.publish(move)
else:
move.linear.x = TURN_RIGHT_SPD
move.angular.z = -1 * ANGULAR_VEL
pub.publish(move)
except CvBridgeError as e:
print(e)
if __name__ == '__main__':
bridge = CvBridge()
rospy.init_node('move_robot')
rospy.Subscriber('/robot/camera/image_raw', Image, callback)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rate = rospy.Rate(2)
rospy.spin()
#### symbolic variables to be replaced with numerics
oldvars = [
'l_f', 'l_r', 'diff(delta_f(t),t)', 'delta_f(t)', 'diff(v(t),t)', 'v(t)',
'diff(psi(t),t)', 'psi(t)', 'diff(a(t),t)', 'a(t)'
] #10 vars to be replaced
newvars = ['l_f', 'l_r', 'dd', 'd', 'dv', 'v', 'dp', 'p', 'da',
'a'] #10 vars to be replaced
for i in range(0, len(oldvars)):
temp = sym.sympify(oldvars[i])
F = F.subs({temp: newvars[i]})
G = G.subs({temp: newvars[i]})
#### NEED TO ADD I AND CONSIDER LOOP RATE. DISCRETE TIME REQUIREMENTS
des_rate = 500.0
rate = rospy.Rate(des_rate)
F = F / des_rate + sym.eye(F.shape[0])
G = G / des_rate
### All "deriviatves" must be bare-bone variables
Fobj = sym.lambdify((newvars), F, "numpy")
Gobj = sym.lambdify((newvars), G, "numpy")
count = 0 # ADAPTIVE KALMAN FILTER COUNTER
eps = 0 # ADAPTIVE KALMAN FILTER EPS
kfwin = 100 # SAVITZKY GOLAY, KF WIND
svgwin = 25 # SVG WIN
svgorder = 3
kfvect = [] # kf vector of size win
svresults = np.array([kfwin * [0]])
timeInit = rospy.get_time()
