def __init__(self):
rospy.on_shutdown(self.cleanup)
# publish command message to joints/servos of arm
self.joint1 = rospy.Publisher('/waist_controller/command',Float64)
self.joint2 = rospy.Publisher('/shoulder_controller/command',Float64)
self.joint3 = rospy.Publisher('/elbow_controller/command',Float64)
self.joint4 = rospy.Publisher('/wrist_controller/command',Float64)
self.joint5 = rospy.Publisher('/hand_controller/command',Float64)
self.pos1 = Float64()
self.pos2 = Float64()
self.pos3 = Float64()
self.pos4 = Float64()
self.pos5 = Float64()
# Initial gesture of robot arm
self.pos1 = 1.565
self.pos2 = 2.102
self.pos3 = -2.439
self.pos4 = -1.294
self.pos5 = 0.0
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
self.joint5.publish(self.pos5)
while not rospy.is_shutdown():
# gesture 1
self.pos1 = 0.559
self.pos2 = 0.215
self.pos3 = -1.508
self.pos4 = -0.496
self.pos5 = 0.0
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
self.joint5.publish(self.pos5)
rospy.sleep(2)
# gesture 2
self.pos1 = 0.565
self.pos2 = -2.393
self.pos3 = -0.639
self.pos4 = 1.335
self.pos5 = -0.430
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
self.joint5.publish(self.pos5)
rospy.sleep(3)
# gesture 1
self.pos1 = 0.559
self.pos2 = 0.215
self.pos3 = -1.508
self.pos4 = -0.496
self.pos5 = 0.0
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
self.joint5.publish(self.pos5)
rospy.sleep(2)
# initial gesture
self.pos1 = 0.565
self.pos2 = 2.102
self.pos3 = -2.439
self.pos4 = -1.294
self.pos5 = 0.0
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
self.joint5.publish(self.pos5)
rospy.sleep(3)
[(50.0, 5.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(50.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(40.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(40.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(30.0, 5.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(30.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(20.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(20.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(10.0, 5.0, 0.0), (0.0, 0.0, 0.0, 1.0)],
[(10.0, 100.0, 0.0), (0.0, 0.0, 0.0, 1.0)]
]
result = Float64()
result.data = 0
x_offset = 150
y_offset = 10
maxSimulations = 1
maxTime = 30*60;
def goal_pose(pose):
goal_pose = Odometry()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'world'
goal_pose.pose.pose.position = Point(pose[0][0]+x_offset, pose[0][1]+y_offset, 0.)
return goal_pose
def get_result(result_aux):
global result
def run(self):
while not rospy.is_shutdown():
self.detectMarker()
if self.data.appear:
self.pub.publish(Float64(self.data.area))
print self.data
def main():
global args
args = build_argparser().parse_args()
model_xml = args.model
model_bin = os.path.splitext(model_xml)[0] + ".bin"
# ------------- 1. Plugin initialization for specified device and load extensions library if specified -------------
log.info("Creating Inference Engine...")
ie = IECore()
if args.cpu_extension and 'CPU' in args.device:
ie.add_extension(args.cpu_extension, "CPU")
# -------------------- 2. Reading the IR generated by the Model Optimizer (.xml and .bin files) --------------------
log.info("Loading network files:\n\t{}\n\t{}".format(model_xml, model_bin))
net = IENetwork(model=model_xml, weights=model_bin)
print("model loaded")
# ---------------------------------- 3. Load CPU extension for support specific layer ------------------------------
if "CPU" in args.device:
supported_layers = ie.query_network(net, "CPU")
not_supported_layers = [
l for l in net.layers.keys() if l not in supported_layers
]
if len(not_supported_layers) != 0:
log.error(
"Following layers are not supported by the plugin for specified device {}:\n {}"
.format(args.device, ', '.join(not_supported_layers)))
log.error(
"Please try to specify cpu extensions library path in sample's command line parameters using -l "
"or --cpu_extension command line argument")
sys.exit(1)
assert len(net.inputs.keys(
)) == 1, "Sample supports only YOLO V3 based single input topologies"
# ---------------------------------------------- 4. Preparing inputs -----------------------------------------------
log.info("Preparing inputs")
input_blob = next(iter(net.inputs))
# Defaulf batch_size is 1
net.batch_size = 1
# Read and pre-process input images
n, c, h, w = net.inputs[input_blob].shape
if args.labels:
with open(args.labels, 'r') as f:
labels_map = [x.strip() for x in f]
else:
labels_map = None
is_async_mode = True #CHG!!!
''' CHG '''
wait_rate = rospy.Rate(1000)
global rgb_img_update, depth_img_update, rgb_image, depth_img, bridge, motor_yaw_queue
pub_total = rospy.Publisher("/yolo_ros_real_pose/detected_objects",
ObjectsRealPose,
queue_size=1)
corresponding_img_pub = rospy.Publisher(
"/yolo_ros_real_pose/img_for_detected_objects", Image, queue_size=1)
pub_time = rospy.Publisher("/yolo_ros_real_pose/detection_time",
Float64,
queue_size=1)
time_this = Float64()
info_this = InfoGather()
info_next = InfoGather()
''' END '''
if args.input == "ROS":
while not rospy.is_shutdown():
if rgb_img_update == 1 and depth_img_update == 1 and len(
rgb_image) != 0 and len(depth_img) != 0 and (
not motor_yaw_queue.empty()
or not motor_yaw_syncronize):
frame = rgb_image.copy()
depth_this = depth_img.copy()
info_this.uav_pose = uav_pose.pose
info_this.img_to_pub = bridge.cv2_to_imgmsg(
rgb_image, encoding="passthrough")
info_this.img_to_pub.header = time_stamp_header
if motor_yaw_syncronize:
info_this.motor_yaw = motor_yaw_queue.get()
else:
info_this.motor_yaw = motor_yaw_corrected
print("Time difference=" +
str(uav_pose.header.stamp.to_sec() -
time_stamp_header.stamp.to_sec()))
rgb_img_update = 0
depth_img_update = 0
wait_key_code = 1
break
else:
input_stream = 0 if args.input == "cam" else args.input
cap = cv2.VideoCapture(input_stream)
number_input_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
number_input_frames = 1 if number_input_frames != -1 and number_input_frames < 0 else number_input_frames
wait_key_code = 1
# Number of frames in picture is 1 and this will be read in cycle. Sync mode is default value for this case
if number_input_frames != 1:
ret, frame = cap.read()
else:
is_async_mode = False
wait_key_code = 0
# ----------------------------------------- 5. Loading model to the plugin -----------------------------------------
log.info("Loading model to the plugin")
exec_net = ie.load_network(network=net,
num_requests=2,
device_name=args.device)
cur_request_id = 0
next_request_id = 1
render_time = 0
parsing_time = 0
# ----------------------------------------------- 6. Doing inference -----------------------------------------------
log.info("Starting inference...")
print(
"To close the application, press 'CTRL+C' here or switch to the output window and press ESC key"
)
print(
"To switch between sync/async modes, press TAB key in the output window"
)
while not rospy.is_shutdown():
# Here is the first asynchronous point: in the Async mode, we capture frame to populate the NEXT infer request
# in the regular mode, we capture frame to the CURRENT infer request
try:
detection_start_time = time()
if args.input == "ROS":
while not rospy.is_shutdown():
if rgb_img_update == 1 and depth_img_update == 1 and (
not motor_yaw_queue.empty()
or not motor_yaw_syncronize):
if len(rgb_image) != 0 and len(depth_img) != 0:
if is_async_mode:
next_frame = rgb_image.copy()
depth_next = depth_img.copy()
info_next.uav_pose = uav_pose.pose
info_next.img_to_pub = bridge.cv2_to_imgmsg(
rgb_image, encoding="passthrough")
info_next.img_to_pub.header = time_stamp_header
if motor_yaw_syncronize:
info_next.motor_yaw = motor_yaw_queue.get()
else:
info_next.motor_yaw = motor_yaw_corrected
else:
frame = rgb_image.copy()
depth_this = depth_img.copy()
info_this.uav_pose = uav_pose.pose
info_this.img_to_pub = bridge.cv2_to_imgmsg(
rgb_image, encoding="passthrough")
info_this.img_to_pub.header = time_stamp_header
if motor_yaw_syncronize:
info_this.motor_yaw = motor_yaw_queue.get()
else:
info_this.motor_yaw = motor_yaw_corrected
print("Time difference=" +
str(uav_pose.header.stamp.to_sec() -
time_stamp_header.stamp.to_sec()))
rgb_img_update = 0
depth_img_update = 0
break
wait_rate.sleep()
else:
if is_async_mode:
ret, next_frame = cap.read()
else:
ret, frame = cap.read()
if not ret:
break
if is_async_mode:
request_id = next_request_id
in_frame = cv2.resize(next_frame, (w, h))
else:
request_id = cur_request_id
in_frame = cv2.resize(frame, (w, h))
# resize input_frame to network size
in_frame = in_frame.transpose(
(2, 0, 1)) # Change data layout from HWC to CHW
in_frame = in_frame.reshape((n, c, h, w))
# Start inference
start_time = time()
exec_net.start_async(request_id=request_id,
inputs={input_blob: in_frame})
det_time = time() - start_time
# Collecting object detection results
objects = list()
if exec_net.requests[cur_request_id].wait(-1) == 0:
output = exec_net.requests[cur_request_id].outputs
start_time = time()
for layer_name, out_blob in output.items():
out_blob = out_blob.reshape(
net.layers[net.layers[layer_name].parents[0]].shape)
layer_params = YoloParams(net.layers[layer_name].params,
out_blob.shape[2])
log.info("Layer {} parameters: ".format(layer_name))
layer_params.log_params()
objects += parse_yolo_region(out_blob, in_frame.shape[2:],
frame.shape[:-1],
layer_params,
args.prob_threshold)
parsing_time = time() - start_time
# Filtering overlapping boxes with respect to the --iou_threshold CLI parameter
objects = sorted(objects,
key=lambda obj: obj['confidence'],
reverse=True)
for i in range(len(objects)):
if objects[i]['confidence'] == 0:
continue
for j in range(i + 1, len(objects)):
if intersection_over_union(
objects[i], objects[j]) > args.iou_threshold:
objects[j]['confidence'] = 0
# Drawing objects with respect to the --prob_threshold CLI parameter
objects = [
obj for obj in objects
if obj['confidence'] >= args.prob_threshold
]
if len(objects) and args.raw_output_message:
log.info("\nDetected boxes for batch {}:".format(1))
log.info(
" Class ID | Confidence | XMIN | YMIN | XMAX | YMAX | COLOR "
)
origin_im_size = frame.shape[:-1]
current_total_dect = ObjectsRealPose()
for obj in objects:
# Validation bbox of detected object
if obj['xmax'] > origin_im_size[1] or obj[
'ymax'] > origin_im_size[0] or obj['xmin'] < 0 or obj[
'ymin'] < 0:
continue
color = (int(min(obj['class_id'] * 12.5,
255)), min(obj['class_id'] * 7,
255), min(obj['class_id'] * 5,
255))
det_label = labels_map[obj['class_id']] if labels_map and len(labels_map) >= obj['class_id'] else \
str(obj['class_id'])
if args.raw_output_message:
log.info(
"{:^9} | {:10f} | {:4} | {:4} | {:4} | {:4} | {} ".
format(det_label, obj['confidence'], obj['xmin'],
obj['ymin'], obj['xmax'], obj['ymax'], color))
cv2.rectangle(frame, (obj['xmin'], obj['ymin']),
(obj['xmax'], obj['ymax']), color, 2)
cv2.putText(
frame, "#" + det_label + ' ' +
str(round(obj['confidence'] * 100, 1)) + ' %',
(obj['xmin'], obj['ymin'] - 7), cv2.FONT_HERSHEY_COMPLEX,
0.6, color, 1)
'''CHG'''
x_pos = 0
y_pos = 0
z_pos = 0
info = RealPose()
info.label = det_label
info.confidence = obj['confidence']
info.pix_lt_x = obj['xmin']
info.pix_lt_y = obj['ymin']
info.pix_rb_x = obj['xmax']
info.pix_rb_y = obj['ymax']
info.head_yaw = info_this.motor_yaw
info.local_pose = info_this.uav_pose
### Now calculate the real position
if len(depth_this) != 0 and (obj['xmax'] - obj['xmin']) * (
obj['ymax'] - obj['ymin']) >= 0:
### Calculate position here by depth image and camera parameters
depth_box_width = obj['xmax'] - obj['xmin']
depth_box_height = obj['ymax'] - obj['ymin']
delta_rate = 0.1
x_box_min = int(obj['xmin'] + depth_box_width * delta_rate)
y_box_min = int(obj['ymin'] +
depth_box_height * delta_rate)
x_box_max = int(obj['xmax'] - depth_box_width * delta_rate)
y_box_max = int(obj['ymax'] -
depth_box_height * delta_rate)
after_width = (depth_box_width * (1 - 2 * delta_rate))
after_height = (depth_box_height * (1 - 2 * delta_rate))
''' '''
# rect = depth_this[y_box_min:y_box_max, x_box_min:x_box_max] # Simulation
rect = depth_this[y_box_min:y_box_max,
x_box_min:x_box_max] * 0.001
rect[np.where(rect == 0)] = 99
rect[np.where(rect != rect)] = 99
z_pos = rect.min()
x_pos = (0.5 *
(obj['xmax'] + obj['xmin']) - cx) * z_pos / fx
y_pos = (0.5 *
(obj['ymax'] + obj['ymin']) - cy) * z_pos / fy
info.x = x_pos
info.y = y_pos
info.z = z_pos
current_total_dect.result.append(info)
if len(current_total_dect.result) > 0:
current_total_dect.header = info_this.img_to_pub.header
pub_total.publish(current_total_dect)
corresponding_img_pub.publish(info_this.img_to_pub)
time_this.data = time() - detection_start_time
pub_time.publish(time_this)
'''END'''
# Draw performance stats over frame
inf_time_message = "Inference time: N\A for async mode" if is_async_mode else \
"Inference time: {:.3f} ms".format(det_time * 1e3)
render_time_message = "OpenCV rendering time: {:.3f} ms".format(
render_time * 1e3)
async_mode_message = "Async mode is on. Processing request {}".format(cur_request_id) if is_async_mode else \
"Async mode is off. Processing request {}".format(cur_request_id)
parsing_message = "YOLO parsing time is {:.3f}".format(
parsing_time * 1e3)
cv2.putText(frame, inf_time_message, (15, 15),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (200, 10, 10), 1)
cv2.putText(frame, render_time_message, (15, 45),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1)
cv2.putText(frame, async_mode_message,
(10, int(origin_im_size[0] - 20)),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1)
cv2.putText(frame, parsing_message, (15, 30),
cv2.FONT_HERSHEY_COMPLEX, 0.5, (10, 10, 200), 1)
start_time = time()
cv2.imshow("DetectionResults", frame)
# cv2.imshow("depth_this", depth_this)
render_time = time() - start_time
''' For next '''
if is_async_mode:
cur_request_id, next_request_id = next_request_id, cur_request_id
frame = next_frame
depth_this = depth_next
info_this.uav_pose = info_next.uav_pose
info_this.motor_yaw = info_next.motor_yaw
info_this.img_to_pub = info_next.img_to_pub
info_this.img_to_pub.header = info_next.img_to_pub.header
key = cv2.waitKey(wait_key_code)
# ESC key
if key == 27:
break
# Tab key
if key == 9:
exec_net.requests[cur_request_id].wait()
is_async_mode = not is_async_mode
log.info("Switched to {} mode".format(
"async" if is_async_mode else "sync"))
except:
print("An error occuered!!!!!!!")
cv2.destroyAllWindows()
#!/usr/bin/env python
from gazebo_msgs.msg import ODEPhysics
from geometry_msgs.msg import Vector3
from gazebo_msgs.srv import SetPhysicsProperties
from std_msgs.msg import Float64
import rospy
set_gravity = rospy.ServiceProxy('/gazebo/set_physics_properties',
SetPhysicsProperties)
time_step = Float64(0.004) # default: 0.001
max_update_rate = Float64(250.0) # default: 1000.0
gravity = Vector3()
gravity.x = 0.0
gravity.y = 0.0
gravity.z = -9.8
ode_config = ODEPhysics()
ode_config.auto_disable_bodies = False
ode_config.sor_pgs_precon_iters = 0
ode_config.sor_pgs_iters = 50
ode_config.sor_pgs_w = 1.3
ode_config.sor_pgs_rms_error_tol = 0.0
ode_config.contact_surface_layer = 0.001
ode_config.contact_max_correcting_vel = 50.0 # default: 100.0
ode_config.cfm = 0.0
ode_config.erp = 0.2
ode_config.max_contacts = 20
print "Changing gazebo properties."
set_gravity(time_step.data, max_update_rate.data, gravity, ode_config)
if __name__ == '__main__':
rospy.init_node('arm_start')
arm1 = rospy.Publisher('arm_1_joint/command', Float64, queue_size=10)
arm2 = rospy.Publisher('arm_2_joint/command', Float64, queue_size=10)
arm3 = rospy.Publisher('arm_3_joint/command', Float64, queue_size=10)
arm4 = rospy.Publisher('arm_4_joint/command', Float64, queue_size=10)
arm5 = rospy.Publisher('arm_5_joint/command', Float64, queue_size=10)
gripper = rospy.Publisher('gripper_1_joint/command', Float64, queue_size=10)
rospy.wait_for_message('joint_states', JointState)
for i in range(5):
arm1.publish(Float64(0.0))
arm2.publish(Float64(0.0))
arm3.publish(Float64(-0.3))
arm4.publish(Float64(-0.65))
gripper.publish(Float64(0.15))
rospy.sleep(0.5)
for i in range(5):
gripper.publish(Float64(0.0))
rospy.sleep(0.5)
for i in range(5):
arm1.publish(Float64(0.0))
arm2.publish(Float64(0.0))
arm3.publish(Float64(0.0))
arm4.publish(Float64(0.0))
gripper.publish(Float64(0.0))
def __init__(self):
rospy.on_shutdown(self.cleanup)
# publish command message to joints/servos of arm
self.joint1 = rospy.Publisher('/arm_shoulder_pan_joint/command',
Float64)
self.joint2 = rospy.Publisher('/arm_shoulder_lift_joint/command',
Float64)
self.joint3 = rospy.Publisher('/arm_elbow_flex_joint/command', Float64)
self.joint4 = rospy.Publisher('/arm_wrist_flex_joint/command', Float64)
self.joint5 = rospy.Publisher('/gripper_joint/command', Float64)
self.ddtg = rospy.Publisher('toggle_msg', Float64)
##############33
self.pub = rospy.Publisher('/armOK', String)
# Define a and b(a is the x-height and b is the y-height)
self.a = Float64()
self.b = Float64()
self.x = Float64()
self.y = Float64()
self.rx = Float64()
self.rz = Float64()
# adding program
self.nav = Float64(
) #the number which is used to get the order from node goforward
# Subscribe three topics to get the x, y, z coordinate
self.x = 10
self.y = 10
self.z = 10
# the variable for diandong tuigan
self.grasp = 0
# adding program
self.nav = 0
self.armsub = 0
self.a2 = 0.105
self.a3 = 0.105
self.a4 = 0.09
self.arm_length = 0.29
def callback_pos(data_pos):
rospy.loginfo(
rospy.get_caller_id() + "I heard that coordinate is %s",
data_pos)
self.grasp = 1
self.x = data_pos.x
self.y = data_pos.y
self.z = data_pos.z
self.rx = self.x
# adding program
def callback_nav(data_nav):
rospy.loginfo("I heard that the robot has reached the place")
self.nav = data_nav
print self.nav
def callback_nav2(data_nav):
if data_nav == "place":
rospy.loginfo("I heard that the robot has reached point B")
self.armsub = 1
else:
self.armsub = 0
# adding program above
rospy.Subscriber("image_pos", pos, callback_pos)
rospy.loginfo("Subscribe to topic pos.....")
rospy.sleep(2)
#adding program
rospy.Subscriber("nav_chatter", Float64, callback_nav)
rospy.loginfo("Subscribe to topic nav_chatter.....")
rospy.sleep(2)
######################
rospy.Subscriber("/nav", String, callback_nav2)
rospy.loginfo("Subscribe to topic image2arm.....")
rospy.sleep(2)
# Define five poses
self.pos1 = Float64()
self.pos2 = Float64()
self.pos3 = Float64()
self.pos4 = Float64()
self.pos5 = Float64()
# Initial gesture of robot arm
self.pos1 = 0 #1.57#4.14
self.pos2 = -1.1
self.pos3 = 2.1
self.pos4 = 1
self.pos5 = -0.4
self.joint1.publish(self.pos1)
rospy.sleep(2)
self.joint5.publish(self.pos5)
rospy.sleep(2)
self.joint4.publish(self.pos4)
rospy.sleep(2)
self.joint3.publish(2.1)
rospy.sleep(2)
self.joint2.publish(self.pos2)
rospy.sleep(2)
self.joint3.publish(self.pos3)
rospy.sleep(2)
rospy.loginfo("Connecting to turtlebot arm....")
while not rospy.is_shutdown():
#rospy.loginfo("Test ......")
if self.grasp == 1:
self.a = self.x #used correct data before
self.b = self.y
self.obj_dis = math.sqrt(self.x**2 + self.y**2 + self.z**2)
while self.arm_length < self.obj_dis:
rospy.loginfo(
"This coordinate is out of my location......")
rospy.sleep(1)
rospy.loginfo("The coordinate x we used to catch is ")
print self.rx
rospy.loginfo("The coordinate z we used to catch is ")
print self.rz
rospy.loginfo("X coordinate is ....") #print x coordinate
print self.a
rospy.loginfo("Y coordinate is ....") #print y coordinate
print self.b
if self.y < 0.27:
self.y = self.y + 0.019
# calculate new joint angles
self.theta1 = -math.atan2(self.x, self.y)
self.l = math.hypot(self.x, self.y) - self.a4
self.l1 = math.hypot(self.l, self.z)
self.cosfai = self.l1 / (2 * self.a2)
rospy.loginfo("cosfai is ....") #print x coordinate
print self.cosfai
self.fai = math.acos(self.cosfai)
self.theta = math.atan2(self.z, self.l)
self.theta2 = math.pi / 2 - self.theta - self.fai
self.theta4 = self.theta2 - math.acos(self.l1**2 /
(2 * self.a2 * self.a3) -
1)
self.theta3 = math.pi / 2 - self.theta2 - self.theta4
self.theta5 = 0.5
self.pos1 = self.theta1
self.pos2 = self.theta2
self.pos3 = self.theta3
self.pos4 = self.theta4
self.pos5 = self.theta5
rospy.loginfo("Calculate new joint angles ......")
rospy.loginfo("The rotation angle of joint one.....")
print self.theta1
rospy.loginfo("The rotation angle of joint two.....")
print self.theta2
rospy.loginfo("The rotation angle of joint three.....")
print self.theta3
rospy.loginfo("The rotation angle of joint four.....")
print self.theta5
rospy.loginfo("The rotation angle of joint five.....")
print self.theta5
# Rotate joint one to get the direction
self.joint1.publish(self.pos1)
rospy.sleep(2)
# Stretched the arm
self.joint5.publish(self.pos5)
rospy.sleep(2)
self.joint4.publish(-0.4)
rospy.sleep(2)
self.joint2.publish(0)
rospy.sleep(2)
self.joint3.publish(0.3)
rospy.sleep(2)
#self.joint3.publish(0.3)
rospy.sleep(2)
self.joint2.publish(self.pos2)
rospy.sleep(2)
self.joint3.publish(self.pos3)
rospy.sleep(2)
self.joint4.publish(self.pos4)
rospy.sleep(2)
# Catch the goods
rospy.loginfo("Catch the goods ......")
# Lift up the goods
self.pos1 = 0 #1.57#4.14
self.pos2 = 1
self.pos3 = -1
self.pos4 = 0
self.pos5 = -0.3
self.joint5.publish(self.pos5)
rospy.sleep(4)
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
rospy.loginfo("Lift up the goods for 5 seconds ......")
rospy.sleep(5) #15 before
self.image = "go"
self.pub.publish(self.image)
rospy.loginfo(
"I have grasped the item and please go to point B to place the item"
)
if self.armsub == 1:
rospy.loginfo("Ready for releasing the goods")
# Release the goods
self.pos1 = 0
self.pos1 = 0.7
self.pos3 = self.theta3
self.pos4 = self.theta4
self.pos5 = 0.6
self.joint1.publish(self.pos1)
rospy.sleep(4)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
rospy.sleep(4)
self.joint5.publish(self.pos5)
rospy.loginfo("Release the goods ......")
rospy.sleep(4)
self.armsub = 0
self.x = 10
self.z = 10
def control_knee():
global t_control
global stimMsg
global update_values
global co_activation
global new_current_quad
global new_current_hams
global control_sel
global pw_min_h
global pw_min_q
global channels_sel
global control_onoff
global control_enable_quad
global control_enable_hams
global ilc_memory
global ilc_i
global Kp_now
global Ki_now
global Kd_now
global ES_A_now
global ES_omega_now
global ES_phase_now
global ES_K_now
global ES_RC_now
global ILC_alpha_now
global ILC_beta_now
global ILC_gama_now
global initTime
# init_variables
t_control = [0, 0]
# init control node
controller = control.Control(rospy.init_node('control', anonymous=False))
# communicate with the dynamic server
dyn_params = reconfig.Client('server', config_callback=server_callback)
# subscribed topics
sub = dict()
sub.update({
'pedal':
rospy.Subscriber('imu/pedal', Imu, callback=imu_callback),
'ref':
rospy.Subscriber('ref_channel', Float64, callback=reference_callback),
'steps':
rospy.Subscriber('steps_channel', Float64, callback=steps_callback),
'curve':
rospy.Subscriber('calibration_channel',
Float64,
callback=curve_callback)
})
# sub = {}
# sub['pedal'] = rospy.Subscriber('imu/pedal', Imu, callback=imu_callback)
# sub['ref'] = rospy.Subscriber('ref_channel', Float64, callback=reference_callback)
# sub['steps'] = rospy.Subscriber('steps_channel', Float64, callback=steps_callback)
# published topics
pub = dict()
pub.update({
'talker':
rospy.Publisher('chatter', String, queue_size=10),
'control':
rospy.Publisher('stimulator/ccl_update', Stimulator, queue_size=10),
'angle':
rospy.Publisher('control/angle', Float64, queue_size=10),
'signal':
rospy.Publisher('control/stimsignal', Int32MultiArray, queue_size=10)
})
# define loop rate (in hz)
rate = rospy.Rate(freq)
# build basic angle message
angleMsg = Float64()
# errMsg = Float64()
# build basic stimulator message
stimMsg = Stimulator()
stimMsg.mode = ['single', 'single', 'single', 'single']
stimMsg.pulse_width = [0, 0, 0, 0]
stimMsg.pulse_current = [0, 0, 0, 0]
stimMsg.channel = [1, 2, 3, 4]
control_sel = 0
pw_min_h = 0
pw_min_q = 0
channels_sel = 0
control_onoff = False
ESC_now = [0, 0, 0, 0, 0]
ILC_now = [0, 0, 0]
co_activation = False
initTime = tempo.time()
# load inverse dynamics
try:
ID_data = sio.loadmat(ID_path)
id_pw = ID_data['id_pw'][0]
id_angle = ID_data['id_angle'][0]
# knee_ref = reference_data['knee_ref'][0]
# loaded = 1
except:
print("Ooops, the file you tried to load is not in the folder.")
# knee_ref = [0, 0, 0, 0]
# ---------------------------------------------------- start
while not rospy.is_shutdown():
thisKnee = angle[-1]
thisError = err_angle[-1]
thisRef = refKnee[-1]
thisTime = tempo.time() - initTime
# ============================== >controllers start here
new_kp = Kp_vector[-1]
new_ki = Ki_vector[-1]
new_kd = Kd_vector[-1]
new_kp_hat = Kp_hat_vector[-1]
new_ki_hat = Ki_hat_vector[-1]
new_kd_hat = Kd_hat_vector[-1]
newIntegralError = 0
new_u = 0
jcost = controller.jfunction(thisError, freq) # cost function
if not co_activation:
co_act_h = 0
co_act_q = 0
else:
co_act_h = pw_min_h
co_act_q = pw_min_q
if control_onoff:
# Open-loop inverse dynamics
if control_sel == 0:
try:
res = next(x for x in id_angle if x > thisRef)
# print list(id_angle).index(res)
except StopIteration:
res = 0
print "Not in recruitment curve"
new_u = res / 500
# if thisError < 0:
# new_u = -1
# else:
# new_u = 1
# PID
elif control_sel == 1:
new_kp = Kp_now
new_ki = Ki_now
new_kd = Kd_now
new_u, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# PID-ILC
elif control_sel == 2:
new_kp = Kp_now
new_ki = Ki_now
new_kd = Kd_now
# first do PID
thisU_PID, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# then ILC
ilc_send[0] = ilc_memory[0][ilc_i]
ilc_send[1] = ilc_memory[1][ilc_i]
# only change new_u after the second cycle
if ilc_memory[0][ilc_i] == 0:
new_u = thisU_PID
else:
ILC_now[0] = ILC_alpha_now
ILC_now[1] = ILC_beta_now
ILC_now[2] = ILC_gama_now
new_u = controller.pid_ilc(ilc_send, ILC_now, thisU_PID,
ilc_i)
if new_u > 1:
new_u = 1
elif new_u < -1:
new_u = -1
# update ilc_memory
ilc_memory[0][ilc_i] = thisError
ilc_memory[1][ilc_i] = new_u
# update counter
if ilc_i < len(ilc_memory[0]) - 1:
ilc_i = ilc_i + 1
else:
ilc_i = 0
# PID-ES
elif control_sel == 3:
# calculate new pid parameters using ES
ESC_now[0] = ES_A_now
ESC_now[1] = ES_omega_now
ESC_now[2] = ES_phase_now
ESC_now[3] = ES_RC_now
ESC_now[4] = ES_K_now
new_kp, new_kp_hat = controller.pid_es2(
jcost, jcost_vector[-1], Kp_vector[-1], Kp_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new kp
ESC_now[0] = ES_A_now / 4 # ES_A_now
# ESC_now[1] = ES_omega_now-2
ESC_now[2] = ES_phase_now + 0.1745
# ESC_now[3] = ES_RC_now/2
# ESC_now[4] = ES_K_now/2
new_ki, new_ki_hat = controller.pid_es2(
jcost, jcost_vector[-1], Ki_vector[-1], Ki_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new ki
ESC_now[0] = ES_A_now / 16 # ES_A_now
# ESC_now[1] = ES_omega_now-3
ESC_now[2] = ES_phase_now + 0.523
# ESC_now[3] = ES_RC_now/8
# ESC_now[4] = ES_K_now/8
new_kd, new_kd_hat = controller.pid_es2(
jcost, jcost_vector[-1], Kd_vector[-1], Kd_hat_vector[-1],
1 / freq, thisTime, ESC_now) # new kd
# do PID
new_u, newIntegralError = controller.pid(
thisError, u[-1], u[-2], integralError[-1], freq,
[new_kp, new_ki, new_kd])
# CR
elif control_sel == 4: # curve recruitment
if control_enable_quad == 1:
new_u = curveRecr[-1]
elif control_enable_hams == 1:
new_u = -curveRecr[-1]
# no controller
else:
print("No controller was selected.")
new_kp = 0
new_ki = 0
new_kd = 0
new_kp_hat = 0
new_ki_hat = 0
new_kd_hat = 0
jcost = 0
newIntegralError = 0
new_u = 0
if new_u > 1:
new_u = 1
elif new_u < -1:
new_u = -1
# update vectors
Kp_vector.append(new_kp)
Ki_vector.append(new_ki)
Kd_vector.append(new_kd)
Kp_hat_vector.append(new_kp_hat)
Ki_hat_vector.append(new_ki_hat)
Kd_hat_vector.append(new_kd_hat)
jcost_vector.append(jcost)
integralError.append(newIntegralError)
u.append(new_u)
# print refKnee[-1]
# ============================== controllers end here
# u to pw
# new_pw = round(abs(u[-1] * 500) / 10) * 10
# define pw for muscles
if u[-1] < 0:
# new_pw = round(abs(u[-1]) * (500-pw_min_h) + pw_min_h) / 10 * 10 # u to pw
new_pw = round(abs(u[-1]) * (500 - pw_min_h) + pw_min_h) # u to pw
pw_h.append(new_pw)
pw_q.append(co_act_q)
controlMsg = "angle: %.3f, error: %.3f, u: %.3f (pw: %.1f) (Q), kp,ki,kd = { %.5f, %.5f, %.5f} : "\
% (thisKnee, thisError, u[-1], pw_q[-1], Kp_vector[-1], Ki_vector[-1], Kd_vector[-1])
else:
new_pw = round(abs(u[-1]) * (500 - pw_min_q) + pw_min_q) # u to pw
pw_q.append(new_pw)
pw_h.append(co_act_h)
controlMsg = "angle: %.3f, error: %.3f, u: %.3f (pw: %.1f) (H), kp,ki,kd = { %.5f, %.5f, %.5f} : "\
% (thisKnee, thisError, u[-1], pw_h[-1], Kp_vector[-1], Ki_vector[-1], Kd_vector[-1])
print(controlMsg)
# define current for muscles
current_q.append(new_current_quad)
current_h.append(new_current_hams)
# update topics
if channels_sel == 1: # right leg
stimMsg.pulse_width = [0, 0, pw_q[-1], pw_h[-1]]
stimMsg.pulse_current = [0, 0, current_q[-1], current_h[-1]]
else: # left leg
stimMsg.pulse_width = [pw_q[-1], pw_h[-1], 0, 0]
stimMsg.pulse_current = [current_q[-1], current_h[-1], 0, 0]
pub['control'].publish(stimMsg) # send stim update
angleMsg.data = thisKnee
pub['angle'].publish(angleMsg) # send imu update
# update timestamp
# ts = tempo.time()
t_control.append(thisTime)
# next iteration
rate.sleep()
# ---------------------------------------------------- save everything
# recreate vectors
angle_err_lists = [angle, err_angle, t_angle]
pid_lists = [Kp_vector, Ki_vector, Kd_vector, integralError, t_control]
stim_lists = [current_q, current_h, pw_q, pw_h, t_control]
ref_lists = [refKnee, t_ref, curveRecr, t_curve]
ilc_lists = [ilc_memory, ILC_now, t_control]
esc_lists = [
ESC_now, t_control, Kp_hat_vector, Ki_hat_vector, Kd_hat_vector,
jcost_vector
]
control_lists = [u, t_control]
gui_lists = ([
t_gui, gui_enable_c, gui_control_sel, gui_step_time, gui_kp, gui_ki,
gui_kd, gui_alpha, gui_beta, gui_gama, gui_A, gui_omega, gui_phase,
gui_channels_sel, gui_pw_min_q, gui_pw_min_h, gui_K, gui_RC
])
path_lists = ([gui_ref_param])
steps_lists = ([t_steps, count_steps])
# create name of the file
currentDT = datetime.datetime.now()
# path_str = "/home/bb8/Desktop/gait_"
path_str = gui_save_param[-1]
if channels_sel == 1: # right leg
leg_str = '_R_'
else: # left leg
leg_str = '_L_'
if control_sel == 0:
control_str = '0_'
elif control_sel == 1:
control_str = '1_'
elif control_sel == 2:
control_str = '2_'
elif control_sel == 3:
control_str = '3_'
elif control_sel == 4:
control_str = '4_'
else:
control_str = 'x_'
if not co_activation:
coact_str = "_noCA"
else:
coact_str = "_CA"
currentDT_str = str(currentDT)
name_file = path_str + coact_str + leg_str + control_str + currentDT_str + ".mat"
# save .mat
sio.savemat(
name_file, {
'angle_err_lists': angle_err_lists,
'pid_lists': pid_lists,
'stim_lists': stim_lists,
'ref_lists': ref_lists,
'ilc_lists': ilc_lists,
'control_lists': control_lists,
'esc_lists': esc_lists,
'gui_lists': gui_lists,
'path_lists': path_lists,
'steps_lists': steps_lists
})
rospy.init_node(name)
host = rospy.get_param('~host')
rate = rospy.get_param('~rate')
ondotori_no = rospy.get_param('~ondotori_no')
name_temp = node_name + ondotori_no + '_temp'
name_hum = node_name + ondotori_no + '_hum'
try:
ondo = tr72w(host)
except OSError as e:
rospy.logerr("{e.strerror}. host={host}".format(**locals()))
sys.exit()
pub_temp = rospy.Publisher(name_temp, Float64, queue_size=1)
pub_hum = rospy.Publisher(name_hum, Float64, queue_size=1)
while not rospy.is_shutdown():
ret = ondo.measure()
msg_temp = Float64()
msg_hum = Float64()
msg_temp.data = float(ret[0])
msg_hum.data = float(ret[1])
pub_temp.publish(msg_temp)
pub_hum.publish(msg_hum)
continue
self._limb.set_joint_positions(joint_angles)
#self._limb.set_joint_velocities(cmd)
diff = np.array(center) - np.array(desired_position)
print('status=[{0},{1}]'.format(center[0],center[1]))
print('desired=[{0},{1}]'.format(desired_position[0],desired_position[1]))
print('desired=[{0},{1}]'.format(diff[0],diff[1]))
if np.linalg.norm(diff) <= 7:
self.step = self.step + 1
self.diff_old = diff
pub_state = rospy.Publisher("/pixel_x/state",Float64,queue_size=1)
msg_state = Float64(center[0])
pub_state.publish(msg_state)
pub_state = rospy.Publisher("/pixel_y/state",Float64,queue_size=1)
msg_state = Float64(center[1])
pub_state.publish(msg_state)
pub_setpoint = rospy.Publisher("/pixel_x/setpoint",Float64,queue_size=1)
msg_setpoint = Float64(desired_position[0])
pub_setpoint.publish(msg_setpoint)
pub_setpoint = rospy.Publisher("/pixel_y/setpoint",Float64,queue_size=1)
msg_setpoint = Float64(desired_position[1])
pub_setpoint.publish(msg_setpoint)
else:
print('search')
current_pose = self._limb.endpoint_pose()
if self.flag == 1:
search_x = 0.5
def publisher():
rospy.init_node('imu')
dataPub = rospy.Publisher('/imu/data', Imu, queue_size=3)
infoPub = rospy.Publisher('/imu/info', bno055_info, queue_size=3)
headingPub = rospy.Publisher('/imu/heading', Float64, queue_size=3)
load_calibration = rospy.get_param("~load_calibration", False)
rate = rospy.Rate(30) #30Hz data read
# Setup BNO055
# Create and configure the BNO sensor connection.
# Raspberry Pi configuration with I2C and RST connected to GPIO 27:
global sensor
sensor = BNO055.BNO055()
try:
sensor.begin()
except Exception as e:
rospy.logerr('Failed to initialize BNO055! %s', e)
sys.exit(1)
#sensor.set_axis_remap(BNO055.AXIS_REMAP_Y, BNO055.AXIS_REMAP_X, BNO055.AXIS_REMAP_Z) #swap x,y axis
# Print system status and self test result.
try:
status, self_test, error = sensor.get_system_status()
except Exception as e:
rospy.logerr('Failed to read BNO055 system status! %s', e)
sys.exit(2)
rospy.logdebug('System status: %s', status)
rospy.logdebug('Self test result (0x0F is normal): %s', hex(self_test))
# Print out an error if system status is in error mode.
if status == 0x01:
rospy.logerr('System error: %s', error)
rospy.logerr('See datasheet section 4.3.59 for the meaning.')
sys.exit(3)
# Print BNO055 software revision and other diagnostic data.
try:
sw, bl, accel, mag, gyro = sensor.get_revision()
except Exception as e:
rospy.logerr('Failed to read BNO055 meta-inforamtion! %s', e)
sys.exit(4)
rospy.loginfo('Software version: %d', sw)
rospy.loginfo('Bootloader version: %d', bl)
rospy.loginfo('Accelerometer ID: %s', hex(accel))
rospy.loginfo('Magnetometer ID: %s', hex(mag))
rospy.loginfo('Gyroscope ID: %s', hex(gyro))
rospy.loginfo('Reading BNO055 data...')
if load_calibration:
try:
sensor.set_calibration(
np.load(
os.path.join(os.path.dirname(os.path.realpath(__file__)),
'calibration.npy')).tolist())
except Exception as e:
rospy.logerr("Error loading calibration data: " + str(e))
save_calibration_srv = rospy.Service('/imu/save_calibration', Trigger,
save_calibration)
while not rospy.is_shutdown():
# Define messages
msg = Imu()
info = bno055_info()
heading = Float64()
orientation = Quaternion()
angular_vel = Vector3()
linear_accel = Vector3()
# Update meta data
attempts = 0
sys_cal = 0
temp_c = 0
while attempts < 4:
try:
# Read the calibration status, 0=uncalibrated and 3=fully calibrated.
sys_cal, gyro, accel, mag = sensor.get_calibration_status()
temp_c = sensor.read_temp()
break
except Exception as e:
rospy.logerr(
'Failed to read BNO055 calibration stat and temp! %s', e)
attempts += 1
rospy.sleep(0.01)
if attempts != 4:
info.sysCalibration = sys_cal
info.accelCalibration = accel
info.gyroCalibration = gyro
info.magnoCalibration = mag
info.tempC = temp_c
# Update real data
attempts = 0
while attempts < 4:
try:
# Orientation as a quaternion:
orientation.x, orientation.y, orientation.z, orientation.w = sensor.read_quaternion(
)
# Gyroscope data (in degrees per second converted to radians per second):
gry_x, gry_y, gry_z = sensor.read_gyroscope()
angular_vel.x = math.radians(gry_x)
angular_vel.y = math.radians(gry_y)
angular_vel.z = math.radians(gry_z)
# Linear acceleration data (i.e. acceleration from movement, not gravity--
# returned in meters per second squared):
linear_accel.x, linear_accel.y, linear_accel.z = sensor.read_linear_acceleration(
)
heading.data = sensor.read_euler()[0]
break
except Exception as e:
rospy.logerr('Failed to read BNO055 data! %s', e)
attempts += 1
rospy.sleep(0.01)
if attempts != 4:
msg.orientation = orientation
msg.orientation_covariance[0] = 0.01 # covariance in x,y,z (3x3)
msg.orientation_covariance[4] = 0.01
msg.orientation_covariance[8] = 0.01
msg.angular_velocity = angular_vel
msg.angular_velocity_covariance[
0] = 0.01 # covariance in x,y,z (3x3)
msg.angular_velocity_covariance[4] = 0.01
msg.angular_velocity_covariance[8] = 0.01
msg.linear_acceleration = linear_accel
msg.linear_acceleration_covariance[
0] = 0.01 # covariance in x,y,z (3x3)
msg.linear_acceleration_covariance[4] = 0.01
msg.linear_acceleration_covariance[8] = 0.01
#update message headers
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'imu_link'
dataPub.publish(msg)
info.header.stamp = rospy.Time.now()
info.header.frame_id = 'imu_link'
infoPub.publish(info)
headingPub.publish(heading)
rate.sleep()
def __init__(self, namespace, arm, slope):
assert slope > 0, 'Slope cannot be negative'
assert len(namespace) > 0, 'Namespace string is empty'
self.sub = {}
self.pub = {}
self.ramp = {}
self.slope = {}
# Stores the last time stamp for the call for a ramp generation
self.last_activation = {}
# Joint limits
self.joint_min = {}
self.joint_max = {}
# Starting time
self.start_time = rospy.get_time()
# Storing the robot's namespace
self.namespace = namespace
# Slope rate for the ramp
self.slope_rate = slope
if self.namespace[0] != '/':
self.namespace = '/' + self.namespace
if self.namespace[-1] != '/':
self.namespace = self.namespace + '/'
activeJointLimits = rospy.get_param(self.namespace + 'arms/' + arm + '/joint_limits')
assert activeJointLimits is not None, 'Joint limits have not been loaded to the parameter server'
self.jointNames = activeJointLimits.keys()
self.jointPos = {}
print 'RampGenerationNode::namespace=', self.namespace
print 'RampGenerationNode::joints=', self.jointNames
self.jointStateSub = rospy.Subscriber(self.namespace + 'joint_states', JointState, self.joint_state_callback)
for joint in activeJointLimits:
self.pub[joint] = rospy.Publisher(self.namespace + arm + '/' + joint + '/controller/command', Float64, queue_size=10)
self.sub[joint] = rospy.Subscriber(self.namespace + arm + '/' + joint + '/ramp_generator/command', numpy_msg(Float64), self.ramp_callback, (joint))
# Set initial joint angle
self.ramp[joint] = Float64()
self.ramp[joint].data = 0.0
# Get joint limits
self.joint_min[joint] = activeJointLimits[joint]['min']
self.joint_max[joint] = activeJointLimits[joint]['max']
assert self.joint_min[joint] < self.joint_max[joint], 'Invalid joint limit, joint=' + joint
self.jointPos[joint] = None
self.slope[joint] = 0.0
self.last_activation[joint] = self.start_time
rate = rospy.Rate(500)
jointPosInit = False
print 'RampGenerationNode::start'
while not rospy.is_shutdown():
# Read initial position of each joint
for joint in self.jointNames:
if self.jointPos[joint] is None and not jointPosInit:
# print 'RampGenerationNode - No joint position available'
continue
elif self.jointPos[joint] is not None and not jointPosInit:
self.ramp[joint].data = self.jointPos[joint]
jointPosInit = True
# If the control input has been deactivated for too long, set
# slope to zero
if rospy.get_time() - self.last_activation[joint] >= 0.5:
self.slope[joint] = 0.0
self.ramp[joint].data = self.slope[joint] * rospy.get_time() + self.ramp[joint].data
if self.ramp[joint].data < self.joint_min[joint]:
self.ramp[joint].data = self.joint_min[joint]
elif self.ramp[joint].data > self.joint_max[joint]:
self.ramp[joint].data = self.joint_max[joint]
self.pub[joint].publish(self.ramp[joint])
rate.sleep()
print 'RampGenerationNode::end'
def pub_valve_angles(self, event):
print 'Valves orientation: ', self.valve_0, ', ', self.valve_1, ', ', self.valve_2, ', ', self.valve_3
self.pub_valve_0.publish(Float64(self.valve_0))
self.pub_valve_1.publish(Float64(self.valve_1))
self.pub_valve_2.publish(Float64(self.valve_2))
self.pub_valve_3.publish(Float64(self.valve_3))
def listener_talker():
inicio = 0 #variable que indica si se leyeron los valores de negro y blanco antes de iniciar los motores
SensorData = sensor_data()
delay_palanca = 3.4
#--------Creacion de los nodos a utilizar----------
rospy.init_node(
'listener_talker_color',
anonymous=True) # Creacion del nodo que se suscribe al sensor de color
pub_motor_L = rospy.Publisher(
'/ev3dev/OutPortA/command', Float64, queue_size=10
) # the node is publishing to the /ev3dev/OutPortA/command topic, motorL velocity
pub_motor_R = rospy.Publisher(
'/ev3dev/OutPortB/command', Float64, queue_size=10
) # the node is publishing to the /ev3dev/OutPortB/command topic, motorR velocity
pub_motor_C = rospy.Publisher(
'/ev3dev/OutPortC/command', Float64, queue_size=10
) # the node is publishing to the /ev3dev/OutPortC/command topic, motorC velocity
rospy.Subscriber(
'color', Illuminance, SensorData.iluminacion
) #To read the color from sensor, the node is subscribed to /color topic
rospy.Subscriber('ultrasonic', Range, SensorData.distancia)
rate = rospy.Rate(10)
#Publicar-----------------------------------------------
while not rospy.is_shutdown():
distancia_m = SensorData.dist
print "distancia", distancia_m
rospy.loginfo(distancia_m)
if distancia_m <= 0.11: # and distancia_m <= 0.125: #the robot detect a box
if SensorData.caja == 0:
pub_motor_L.publish(Float64(0)) # The velocity of motor A
pub_motor_R.publish(Float64(0)) # The velocity of motor B
rospy.sleep(.5)
# baja palanca
pub_motor_C.publish(Float64(-1)) # The velocity of motor B
rospy.sleep(delay_palanca)
pub_motor_C.publish(Float64(0)) # The velocity of motor B
rospy.sleep(1)
# Avanza hacia adelante
pub_motor_R.publish(Float64(1)) # The velocity of motor B
pub_motor_L.publish(Float64(1)) # The velocity of motor A
rospy.sleep(2)
pub_motor_L.publish(Float64(0)) # The velocity of motor A
pub_motor_R.publish(Float64(0)) # The velocity of motor B
rospy.sleep(1)
# sube la palanca
pub_motor_C.publish(Float64(1)) # The velocity of motor C
rospy.sleep(delay_palanca)
pub_motor_C.publish(Float64(0)) # The velocity of motor C
rospy.sleep(1)
# gira hacia izquierda
pub_motor_L.publish(Float64(-1.4)) # The velocity of motor A
pub_motor_R.publish(Float64(1.4)) # The velocity of motor B
rospy.sleep(2)
SensorData.caja = 1
else:
pub_motor_L.publish(Float64(0)) # The velocity of motor A
pub_motor_R.publish(Float64(0)) # The velocity of motor B
rospy.sleep(.5)
# baja palanca
pub_motor_C.publish(Float64(-1)) # The velocity of motor B
rospy.sleep(delay_palanca)
pub_motor_C.publish(Float64(0)) # The velocity of motor B
rospy.sleep(1)
# retrocede
pub_motor_L.publish(Float64(-1)) # The velocity of motor A
pub_motor_R.publish(Float64(-1)) # The velocity of motor B
rospy.sleep(1.7)
pub_motor_L.publish(Float64(0)) # The velocity of motor A
pub_motor_R.publish(Float64(0)) # The velocity of motor B
rospy.sleep(0.5)
# sube la palanca
pub_motor_C.publish(Float64(1)) # The velocity of motor C
rospy.sleep(delay_palanca)
pub_motor_C.publish(Float64(0)) # The velocity of motor C
rospy.sleep(1)
# gira hacia izquierda
pub_motor_L.publish(Float64(-1.4)) # The velocity of motor A
pub_motor_R.publish(Float64(1.4)) # The velocity of motor B
rospy.sleep(2)
SensorData.caja = 0
else:
motor_v_L, motor_v_R = motors_velocity(SensorData.il,
SensorData.dist)
rospy.loginfo(motor_v_L)
rospy.loginfo(motor_v_R)
pub_motor_L.publish(Float64(motor_v_L)) # The velocity of motor A
pub_motor_R.publish(Float64(motor_v_R)) # The velocity of motor B
rate.sleep()
#-------------------------------------------------------
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
servo_pub = rospy.Publisher(servo_state_topic, Float64, queue_size=1)
vesc_state_pub = rospy.Publisher(motor_state_topic,
VescStateStamped,
queue_size=1)
kmm = KinematicMotionModel(motor_state_topic, servo_state_topic,
speed_to_erpm_offset, speed_to_erpm_gain,
steering_angle_to_servo_offset,
steering_angle_to_servo_gain, car_length,
particles)
# Give time to get setup
rospy.sleep(1.0)
# Send initial position and vesc state
servo_msg = Float64()
servo_msg.data = steering_angle_to_servo_gain * TEST_STEERING_ANGLE + steering_angle_to_servo_offset
servo_pub.publish(servo_msg)
rospy.sleep(1.0)
vesc_msg = VescStateStamped()
vesc_msg.header.stamp = rospy.Time.now()
vesc_msg.state.speed = speed_to_erpm_gain * TEST_SPEED + speed_to_erpm_offset
vesc_state_pub.publish(vesc_msg)
rospy.sleep(TEST_DT)
vesc_msg.header.stamp = rospy.Time.now()
vesc_state_pub.publish(vesc_msg)
def people_callback(self, people):
assert len(people.people) == 1
person = people.people[0]
predictions = PeoplePrediction()
markers = MarkerArray()
markers.markers = []
predictions.predicted_people = []
# if the message stamp is empty, we assign the current time
if people.header.stamp == rospy.Time():
people.header.stamp = rospy.get_rostime()
#delta_t = []
tdist = 0.0
for goal in self.goals:
goal.update_distance(person)
dist = -(goal.heuristic_distance[-1] - goal.heuristic_distance[0])
dist = max(dist, 0.0)
tdist += dist**2.0
#delta_t.append(dist)
if tdist > 0.0: # to avoid singularities
for goal in self.goals:
dist = -(goal.heuristic_distance[-1] -
goal.heuristic_distance[0])
dist = max(dist, 0.0)
goal.probability = dist**2.0 / tdist
#print(tdist, [goal.probability for goal in self.goals], sum([goal.probability for goal in self.goals]))
#if sum(delta_t) > 0.0:
# print(tdist, delta_t, sum(delta_t), delta_t[0]/sum(delta_t))
#else:
# print(tdist, delta_t, sum(delta_t))
predictions.path_probabilities = probabilities = []
for goal in self.goals:
goal.predict_path(person)
probabilities.append(Float64())
probabilities[-1].data = goal.probability
k = 0
for i in range(self.num_predictions):
people_one_timestep = People()
people_one_timestep.people = []
for goal in self.goals[::-1]:
person_prediction = goal.path_prediction[i]
people_one_timestep.people.append(person_prediction)
prediction_marker = self.get_marker()
prediction_marker.header.frame_id = people.header.frame_id
prediction_marker.header.stamp = people.header.stamp
prediction_marker.id = k
k += 1
prediction_marker.pose.position = person_prediction.position
# the opacity of the marker is adjusted according to the prediction step
prediction_marker.color.a = (
1 - i / float(self.num_predictions)) * goal.probability
markers.markers.append(prediction_marker)
# fill the header
people_one_timestep.header.frame_id = people.header.frame_id
people_one_timestep.header.stamp = people.header.stamp + \
rospy.Duration(i * self.time_resolution)
# push back the predictions for this time step to the prediction container
predictions.predicted_people.append(people_one_timestep)
self.prediction_pub.publish(predictions)
self.marker_pub.publish(markers)
def move_joints(self, roll_speed):
joint_speed_value = Float64()
joint_speed_value.data = roll_speed
rospy.loginfo("Single Disk Roll Velocity>>"+str(joint_speed_value))
self._roll_vel_pub.publish(joint_speed_value)
def __init__(self):
rospy.on_shutdown(self.cleanup)
# publish command message to joints/servos of arm
self.joint1 = rospy.Publisher('/arm_shoulder_pan_joint/command',
Float64)
self.joint2 = rospy.Publisher('/arm_shoulder_lift_joint/command',
Float64)
self.joint3 = rospy.Publisher('/arm_elbow_flex_joint/command', Float64)
self.joint4 = rospy.Publisher('/arm_wrist_flex_joint/command', Float64)
self.joint5 = rospy.Publisher('/gripper_joint/command', Float64)
#self.ddtg = rospy.Publisher('toggle_msg',Float64)
##############33
self.grasped_pub = rospy.Publisher('/arm_grasped', String)
self.released_pub = rospy.Publisher('/arm_released', String)
# the variable for diandong tuigan
self.grasp = -1
# adding program
self.armsub = -1
def callback_nav(data_nav):
if data_nav.data.find('release') > -1:
rospy.loginfo("I heard that the robot has reached point B")
self.armsub = 1
def followStopCallback(data_stop):
if data_stop.data.find('follow_stop') > -1:
rospy.loginfo("I heard that the robot has reached point A")
self.grasp = 1
# adding program above
rospy.Subscriber("ifFollowme", String, followStopCallback)
rospy.Subscriber("nav2image", String, callback_nav)
rospy.loginfo("Subscribe to topic nav_chatter.....")
rospy.sleep(2)
# Define five poses
self.pos1 = Float64()
self.pos2 = Float64()
self.pos3 = Float64()
self.pos4 = Float64()
self.pos5 = Float64()
# Initial gesture of robot arm
self.pos1 = 1.4 #1.57#4.14
self.pos2 = 1.57
self.pos3 = -1.57
self.pos4 = 0.6
self.pos5 = -0.5
self.joint5.publish(self.pos5)
rospy.sleep(1)
self.joint4.publish(self.pos4)
rospy.sleep(1)
self.joint3.publish(self.pos3)
rospy.sleep(1)
#self.joint2.publish(0.55)
#rospy.sleep(1)
self.joint1.publish(self.pos1)
rospy.sleep(1)
self.joint2.publish(self.pos2)
rospy.sleep(1)
rospy.loginfo("Connecting to turtlebot arm....")
while not rospy.is_shutdown():
#rospy.loginfo("Test ......")
if self.grasp == 1 and self.armsub == -1:
# Rotate joint one to get the direction
self.joint2.publish(1)
rospy.sleep(1)
self.joint1.publish(0)
rospy.sleep(1)
# Stretched the arm
self.joint5.publish(0.6)
rospy.sleep(2)
self.joint3.publish(-1.0)
rospy.sleep(2)
self.joint4.publish(-0.2)
rospy.sleep(10)
self.joint5.publish(-0.3)
rospy.sleep(2)
self.joint2.publish(2.0)
self.joint3.publish(-2.0)
rospy.sleep(2)
#self.joint3.publish(0.3)
rospy.loginfo("Lift up the goods for 5 seconds ......")
self.ifgrasped = "grasped"
self.grasped_pub.publish(self.ifgrasped)
rospy.loginfo(
"I have grasped the item and please go to point B to place the item"
)
self.grasp = 0
if self.armsub == 1 and self.grasp == 0:
rospy.loginfo("Ready for releasing the goods")
# Release the goods
self.pos1 = 0
self.pos2 = 1.4
self.pos3 = 1.2
self.pos4 = 1.2
self.pos5 = 0.6
self.joint1.publish(self.pos1)
rospy.sleep(1)
self.joint2.publish(self.pos2)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
rospy.sleep(1)
self.joint5.publish(self.pos5)
rospy.loginfo("Release the goods ......")
rospy.sleep(3)
self.armsub = 0
self.grasp = -1
self.ifreleased = "released"
self.released_pub.publish(self.ifreleased)
#reset the arm
self.pos1 = 1.4 #1.57#4.14
self.pos2 = 1.57
self.pos3 = -1.57
self.pos4 = 0.6
self.pos5 = -0.5
self.joint5.publish(self.pos5)
rospy.sleep(1)
self.joint4.publish(self.pos4)
rospy.sleep(1)
self.joint3.publish(self.pos3)
rospy.sleep(1)
#self.joint2.publish(0.55)
#rospy.sleep(1)
self.joint1.publish(self.pos1)
rospy.sleep(1)
self.joint2.publish(self.pos2)
rospy.sleep(1)
def calculateTwistCommand(self):
lad = 0.0 #look ahead distance accumulator
targetIndex = len(self.currentWaypoints.waypoints) - 1
for i in range(len(self.currentWaypoints.waypoints)):
if ((i + 1) < len(self.currentWaypoints.waypoints)):
this_x = self.currentWaypoints.waypoints[
i].pose.pose.position.x
this_y = self.currentWaypoints.waypoints[
i].pose.pose.position.y
next_x = self.currentWaypoints.waypoints[
i + 1].pose.pose.position.x
next_y = self.currentWaypoints.waypoints[
i + 1].pose.pose.position.y
lad = lad + math.hypot(next_x - this_x, next_y - this_y)
if (lad > HORIZON):
targetIndex = i + 1
break
targetWaypoint = self.currentWaypoints.waypoints[targetIndex]
targetSpeed = self.currentWaypoints.waypoints[0].twist.twist.linear.x
targetX = targetWaypoint.pose.pose.position.x
targetY = targetWaypoint.pose.pose.position.y
currentX = self.currentPose.pose.position.x
currentY = self.currentPose.pose.position.y
print '[', targetX, targetY, ']'
#get vehicle yaw angle
quanternion = (self.currentPose.pose.orientation.x,
self.currentPose.pose.orientation.y,
self.currentPose.pose.orientation.z,
self.currentPose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quanternion)
yaw = euler[2]
#get angle difference
alpha = math.atan2(targetY - currentY, targetX - currentX) - yaw
print 'alpha = ', alpha
l = math.sqrt(
math.pow(currentX - targetX, 2) + math.pow(currentY - targetY, 2))
if (l > 0.5):
theta = math.atan(2 * L * math.sin(alpha) / l)
print 'theta = ', theta
self.ackermann_steering_control(targetSpeed, -theta)
# #get twist command
left_steer = Float64()
right_steer = Float64()
left_vel = Float64()
right_vel = Float64()
left_steer.data = left_angle
right_steer.data = right_angle
left_vel.data = left_speed
right_vel.data = right_speed
else:
left_steer = Float64()
right_steer = Float64()
left_vel = Float64()
right_vel = Float64()
left_steer.data = 0
right_steer.data = 0
left_vel.data = 0
right_vel.data = 0
self.left_vel_pub.publish(left_vel)
self.right_vel_pub.publish(right_vel)
self.left_steer_pub.publish(left_steer)
self.right_steer_pub.publish(right_steer)
def main(args):
if len(sys.argv) < 2:
print 'Please specify gps file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
gps = np.loadtxt(sys.argv[1], delimiter=",")
utimes = gps[:, 0]
modes = gps[:, 1]
num_satss = gps[:, 2]
lats = gps[:, 3]
lngs = gps[:, 4]
alts = gps[:, 5]
tracks = gps[:, 6]
speeds = gps[:, 7]
bag = rosbag.Bag(sys.argv[2], 'w')
try:
for i, utime in enumerate(utimes):
timestamp = rospy.Time.from_sec(utime / 1e6)
status = NavSatStatus()
if modes[i] == 0 or modes[i] == 1:
status.status = NavSatStatus.STATUS_NO_FIX
else:
status.status = NavSatStatus.STATUS_FIX
status.service = NavSatStatus.SERVICE_GPS
num_sats = UInt16()
num_sats.data = num_satss[i]
fix = NavSatFix()
fix.status = status
fix.latitude = np.rad2deg(lats[i])
fix.longitude = np.rad2deg(lngs[i])
fix.altitude = alts[i]
track = Float64()
track.data = tracks[i]
speed = Float64()
speed.data = speeds[i]
bag.write('fix', fix, t=timestamp)
bag.write('track', track, t=timestamp)
bag.write('speed', speed, t=timestamp)
finally:
bag.close()
return 0
def callback(data):
vel_bl_msg = Float64()
vel_br_msg = Float64()
vel_fl_msg = Float64()
vel_fr_msg = Float64()
position_x = data.pose.pose.position.x
position_y = data.pose.pose.position.y
print(position_x, ":", position_y)
velocity_bl = 0
velocity_br = 0
if position_x < 0.65:
velocity_bl = 0
velocity_br = 0
time.sleep(1)
velocity_bl = 4
velocity_br = 4
time.sleep(0.1)
velocity_bl = -8
velocity_br = -3
elif position_y < 0.35:
velocity_bl = 0
velocity_br = 0
time.sleep(1)
velocity_bl = 4
velocity_br = 4
time.sleep(0.1)
velocity_bl = -8
velocity_br = -3
elif position_x > 2.4:
velocity_bl = 0
velocity_br = 0
time.sleep(1)
velocity_bl = 4
velocity_br = 4
time.sleep(0.1)
velocity_bl = -3
velocity_br = -8
elif position_y > 2.2:
velocity_bl = 0
velocity_br = 0
time.sleep(1)
velocity_bl = 4
velocity_br = 4
time.sleep(0.1)
velocity_bl = -3
velocity_br = -8
else:
velocity_bl = random.uniform(-0.1, -10)
velocity_br = random.uniform(-0.1, -10)
time.sleep(0.5)
velocity_fl = velocity_bl-1.0
velocity_fr = velocity_br-1.0
vel_bl_msg.data = velocity_bl
vel_br_msg.data = velocity_br
vel_fl_msg.data = velocity_fl
vel_fr_msg.data = velocity_fr
velocity_bl_publisher.publish(vel_fl_msg)
velocity_br_publisher.publish(vel_fr_msg)
velocity_fl_publisher.publish(vel_fl_msg)
velocity_fr_publisher.publish(vel_fr_msg)
def node():
LIMIT_SWITCH_INIT_VEL = -15000 # counts per second
print("Node starting...")
global bot, delta_kin, lim_subs, limit_flags
rospy.init_node('inverse_kinematics', anonymous=False)
out_of_the_way_pub = rospy.Publisher('delta_out_of_the_way',
Bool,
queue_size=10)
out_of_the_way_pub.publish(Bool(False))
bot = Bot()
#bot.full_init(reset=False, k_p=400, k_d=60)
#bot.set_gains(400, 60, perm=True)
for axis in bot.axes:
axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
axis.controller.vel_setpoint = 0
# bot.full_init()
# for i in range(3):
# bot.test_one(i, mytime=.1)
delta_kin = kin.deltaSolver(realBot=realBot)
# Limit switch init begins here
limit_flags = [False for _ in range(3)]
lim_subs = [
rospy.Subscriber('lim%d' % i, Bool, callback_lim_init(i))
for i in range(3)
]
print("Beginning limit switch initialization...")
for axis in bot.axes:
axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
axis.controller.vel_setpoint = LIMIT_SWITCH_INIT_VEL
while not all(limit_flags):
rospy.sleep(.001)
print("Limit switch initialization complete.")
for axis in bot.axes:
axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
axis.controller.vel_setpoint = 0
for axis in bot.axes:
axis.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL
pub = rospy.Publisher('delta_position', PointStamped, queue_size=10)
moving_pub = rospy.Publisher('delta_moving', Bool, queue_size=10)
degrees_off_pub = rospy.Publisher('degrees_off', Point, queue_size=10)
rospy.Subscriber('desired_position', PointStamped, move_to_point)
current_pubs = [
rospy.Publisher('current%d' % i, Float64, queue_size=100)
for i, _ in enumerate(bot.axes)
]
out_of_the_way_pub = rospy.Publisher('delta_out_of_the_way',
Bool,
queue_size=10)
out_of_the_way_point = PointStamped()
out_of_the_way_point.point = Point(165, 75, -810)
out_of_the_way_point.header.stamp = rospy.Time.now()
out_of_the_way_point.header.frame_id = "robot_base_plate"
move_to_point(out_of_the_way_point)
send_robot_transform()
rate = rospy.Rate(10)
rospy.sleep(1)
out_of_the_way_pub.publish(Bool(True))
while not rospy.is_shutdown():
bot.queueHandle()
thetas = bot.get_rad_all()
x, y, z = delta_kin.FK(thetas)
pos = PointStamped()
pos.point = Point(x=x, y=y, z=z)
pos.header.stamp = rospy.Time.now()
pos.header.frame_id = "robot_base_plate"
pub.publish(pos)
moving_pub.publish(Bool(bot.moving()))
degrees_off_pub.publish(Point(*bot.get_enc_deg_dif_all()))
for i, p in enumerate(current_pubs):
p.publish(Float64(bot.axes[i].motor.current_control.Iq_measured))
rate.sleep()
def __init__(self):
rospy.on_shutdown(self.cleanup)
# publish command message to joints/servos of arm
self.joint1 = rospy.Publisher('/arm_shoulder_pan_joint/command',Float64)
self.joint2 = rospy.Publisher('/arm_shoulder_lift_joint/command',Float64)
self.joint3 = rospy.Publisher('/arm_elbow_flex_joint/command',Float64)
self.joint4 = rospy.Publisher('/arm_wrist_flex_joint/command',Float64)
self.joint5 = rospy.Publisher('/gripper_joint/command',Float64)
#self.ddtg = rospy.Publisher('toggle_msg',Float64)
##############33
self.arm_state_pub = rospy.Publisher('/arm_state', String)
# Subscribe three topics to get the x, y, z coordinate
self.x = 10
self.y = 10
self.z = 10
# the variable for diandong tuigan
self.grasp = 0
# adding program
self.armsub = 0
self.a2 = 0.105
self.a3 = 0.105
self.a4 = 0.09
self.arm_length = 0.3
self.label = -1
def callback_pos (data_pos):
rospy.loginfo(rospy.get_caller_id() + "I heard that coordinate is %s", data_pos)
self.grasp = 1
self.x = data_pos.x
self.y = data_pos.y
self.z = data_pos.z
# adding program
def callback_img (data_img):#if begin to grasp or release,from img progam
if data_img.data.find('release') > -1:
rospy.loginfo( "I heard that the robot has reached cupboard")
self.armsub = 1
else:
self.armsub = 0
def callback_label (data_label):#if begin to grasp or release,from img progam
if data_label.data.find('zero') > -1:
self.label = 0
if data_label.data.find('one') > -1:
self.label = 1
if data_label.data.find('two') > -1:
self.label = 2
if data_label.data.find('three') > -1:
self.label = 3
else:
self.label = -1
print data_label.data
# adding program above
rospy.Subscriber("image_pos", pos, callback_pos)
rospy.loginfo("Subscribe to topic pos.....")
rospy.sleep(1)
#adding program
rospy.Subscriber("obj_label", String, callback_label)
rospy.loginfo("Subscribe to topic obj_label.....")
rospy.sleep(1)
######################
rospy.Subscriber("img2arm_release", String, callback_img)
rospy.loginfo("Subscribe to topic image2arm.....")
rospy.sleep(1)
# Define five poses
self.pos1 = Float64()
self.pos2 = Float64()
self.pos3 = Float64()
self.pos4 = Float64()
self.pos5 = Float64()
# Initial gesture of robot arm
self.pos1 = 1.5#1.57#4.14
self.pos2 = 1.6
self.pos3 = -1.57
self.pos4 = 0.6
self.pos5 = -0.5
self.joint5.publish(self.pos5)
rospy.sleep(1)
self.joint4.publish(self.pos4)
rospy.sleep(1)
self.joint3.publish(self.pos3)
rospy.sleep(1)
#self.joint2.publish(0.55)
#rospy.sleep(1)
self.joint1.publish(self.pos1)
rospy.sleep(1)
self.joint2.publish(self.pos2)
rospy.sleep(1)
rospy.loginfo("Connecting to turtlebot arm....")
while not rospy.is_shutdown():
if self.grasp == 1:
self.obj_dis =math.sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)
while self.arm_length < self.obj_dis:
rospy.loginfo("This coordinate is out of my location......")
rospy.sleep(1)
rospy.loginfo("The coordinate x we used to catch is ")
print self.x
rospy.loginfo("Y coordinate is ....") #print y coordinate
print self.y
rospy.loginfo("The coordinate z we used to catch is ")
print self.z
#if self.y < 0.25:
# self.y = self.y + 0.019
# calculate new joint angles
self.theta1 = - math.atan2(self.x,self.y) + 0.05
self.l = math.hypot(self.x,self.y) - self.a4
self.l1 = math.hypot(self.l,self.z)
print self.l1
self.cosfai = self.l1 / (2 * self.a2)
rospy.loginfo("cosfai is ....") #print x coordinate
print self.cosfai
if self.cosfai > 1:
self.cosfai = 1
self.fai = math.acos(self.cosfai)
self.theta = math.atan2(self.z,self.l)
self.theta2 = math.pi / 2 - self.theta - self.fai
self.theta4 = self.theta2 - math.acos(self.l1 ** 2 / (2 * self.a2 * self.a3) - 1 )
self.theta3 = math.pi / 2 - self.theta2 - self.theta4
self.theta5 = 0.8
self.pos1 = self.theta1
self.pos2 = self.theta2
self.pos3 = self.theta3
self.pos4 = self.theta4
self.pos5 = self.theta5
rospy.loginfo("Calculate new joint angles ......")
rospy.loginfo("The rotation angle of joint one.....")
print self.theta1
rospy.loginfo("The rotation angle of joint two.....")
print self.theta2
rospy.loginfo("The rotation angle of joint three.....")
print self.theta3
rospy.loginfo("The rotation angle of joint four.....")
print self.theta5
rospy.loginfo("The rotation angle of joint five.....")
print self.theta5
# Rotate joint one to get the direction
self.joint2.publish(1.57)
rospy.sleep(1)
# Stretched the arm
self.joint4.publish(-1.57)
rospy.sleep(3)
self.joint3.publish(-1.2)
rospy.sleep(3)
# Stretched the arm
self.joint1.publish(self.pos1)
rospy.sleep(3)
self.joint5.publish(self.pos5)
rospy.sleep(2)
self.joint2.publish(self.pos2)
rospy.sleep(2)
self.joint3.publish(self.pos3)
rospy.sleep(2)
self.joint4.publish(self.pos4)
rospy.sleep(3)
# Catch the goods
rospy.loginfo("Catch the goods ......")
# Lift up the goods
self.pos1 = 0#1.57#4.14
self.pos2 = 1.2
self.pos3 =-1.3
self.pos4 = -0.2
print self.label
if self.label == 0:
self.pos5 = -0.2
if self.label == 1:
self.pos5 = -0.2
if self.label == 2:
self.pos5 = 0
if self.label == 3:
self.pos5 = -0.2
else :
self.pos5 = -0.25
self.joint1.publish(self.pos1)
self.joint2.publish(self.pos2)
self.joint5.publish(self.pos5)
rospy.sleep(5)
self.joint3.publish(self.pos3)
self.joint4.publish(self.pos4)
rospy.loginfo("Lift up the goods for 5 seconds ......")
rospy.sleep(5) #15 before
self.ifgrasped = "grasped"
self.arm_state_pub.publish(self.ifgrasped)
rospy.loginfo("I have grasped the item and please go to point B to place the item")
self.grasp = 0
if self.armsub == 1:
rospy.loginfo("Ready for releasing the goods")
# Release the goods
self.pos1 = 0
self.pos2 = 0.8
self.pos3 = -0.7
self.pos4 = 1.2
self.pos5 = 0.6
self.joint1.publish(self.pos1)
rospy.sleep(1)
self.joint2.publish(self.pos2)
rospy.sleep(1)
self.joint3.publish(self.pos3)
rospy.sleep(1)
self.joint4.publish(self.pos4)
rospy.sleep(1)
self.joint5.publish(self.pos5)
rospy.loginfo("Release the goods ......")
rospy.sleep(5)
self.armsub = 0
self.ifreleased = "released"
self.arm_state_pub.publish(self.ifreleased)
#reset the arm
self.pos1 = Float64()
self.pos2 = Float64()
self.pos3 = Float64()
self.pos4 = Float64()
self.pos5 = Float64()
# Initial gesture of robot arm
self.pos1 = 1.5#1.57#4.14
self.pos2 = 1.57
self.pos3 = -1.57
self.pos4 = 0.6
self.pos5 = -0.5
self.joint5.publish(self.pos5)
rospy.sleep(1)
self.joint4.publish(self.pos4)
rospy.sleep(1)
self.joint3.publish(self.pos3)
rospy.sleep(1)
#self.joint2.publish(0.5)
#rospy.sleep(1)
self.joint1.publish(self.pos1)
rospy.sleep(1)
self.joint2.publish(self.pos2)
rospy.sleep(1)
rospy.loginfo("reset turtlebot arm....")
#!/usr/bin/env python
import rospy
from std_msgs.msg import Float64
rospy.init_node("min_pub")
pub = rospy.Publisher("topic1", Float64, queue_size=1)
rate = rospy.Rate(1)
msg = Float64()
msg.data = 0.0
while not rospy.is_shutdown():
pub.publish(msg)
msg.data = msg.data + 0.1
rate.sleep()
def __init__(self):
#### GoPiGo3 power management
# export pin
if not os.path.isdir("/sys/class/gpio/gpio"+self.POWER_PIN):
gpio_export = os.open("/sys/class/gpio/export", os.O_WRONLY)
os.write(gpio_export, self.POWER_PIN.encode())
os.close(gpio_export)
time.sleep(0.1)
# set pin direction
gpio_direction = os.open("/sys/class/gpio/gpio"+self.POWER_PIN+"/direction", os.O_WRONLY)
os.write(gpio_direction, "out".encode())
os.close(gpio_direction)
# activate power management
self.gpio_value = os.open("/sys/class/gpio/gpio"+self.POWER_PIN+"/value", os.O_WRONLY)
os.write(self.gpio_value, "1".encode())
# GoPiGo3 and ROS setup
self.g = gopigo3.GoPiGo3()
print("GoPiGo3 info:")
print("Manufacturer : ", self.g.get_manufacturer())
print("Board : ", self.g.get_board())
print("Serial Number : ", self.g.get_id())
print("Hardware version: ", self.g.get_version_hardware())
print("Firmware version: ", self.g.get_version_firmware())
self.reset_odometry()
rospy.init_node("gopigo3")
self.br = TransformBroadcaster()
# subscriber
rospy.Subscriber("motor/dps/left", Int16, lambda msg: self.g.set_motor_dps(self.ML, msg.data))
rospy.Subscriber("motor/dps/right", Int16, lambda msg: self.g.set_motor_dps(self.MR, msg.data))
rospy.Subscriber("motor/pwm/left", Int8, lambda msg: self.g.set_motor_power(self.ML, msg.data))
rospy.Subscriber("motor/pwm/right", Int8, lambda msg: self.g.set_motor_power(self.MR, msg.data))
rospy.Subscriber("motor/position/left", Int16, lambda msg: self.g.set_motor_position(self.ML, msg.data))
rospy.Subscriber("motor/position/right", Int16, lambda msg: self.g.set_motor_position(self.MR, msg.data))
rospy.Subscriber("servo/1", Float64, lambda msg: self.g.set_servo(self.S1, msg.data*16666))
rospy.Subscriber("servo/2", Float64, lambda msg: self.g.set_servo(self.S2, msg.data*16666))
rospy.Subscriber("cmd_vel", Twist, self.on_twist)
rospy.Subscriber("led/blinker/left", UInt8, lambda msg: self.g.set_led(self.BL, msg.data))
rospy.Subscriber("led/blinker/right", UInt8, lambda msg: self.g.set_led(self.BR, msg.data))
rospy.Subscriber("led/eye/left", ColorRGBA, lambda c: self.g.set_led(self.EL, int(c.r*255), int(c.g*255), int(c.b*255)))
rospy.Subscriber("led/eye/right", ColorRGBA, lambda c: self.g.set_led(self.ER, int(c.r*255), int(c.g*255), int(c.b*255)))
rospy.Subscriber("led/wifi", ColorRGBA, lambda c: self.g.set_led(self.EW, int(c.r * 255), int(c.g * 255), int(c.b * 255)))
# publisher
self.pub_enc_l = rospy.Publisher('motor/encoder/left', Float64, queue_size=10)
self.pub_enc_r = rospy.Publisher('motor/encoder/right', Float64, queue_size=10)
self.pub_battery = rospy.Publisher('battery_voltage', Float64, queue_size=10)
self.pub_motor_status = rospy.Publisher('motor/status', MotorStatusLR, queue_size=10)
self.pub_odometry = rospy.Publisher("odometry", Odometry, queue_size=10)
# services
self.srv_reset = rospy.Service('reset', Trigger, self.reset)
self.srv_spi = rospy.Service('spi', SPI, lambda req: SPIResponse(data_in=self.g.spi_transfer_array(req.data_out)))
self.srv_pwr_on = rospy.Service('power/on', Trigger, self.power_on)
self.srv_pwr_off = rospy.Service('power/off', Trigger, self.power_off)
# main loop
rate = rospy.Rate(10) # in Hz
while not rospy.is_shutdown():
self.pub_enc_l.publish(Float64(data=self.g.get_motor_encoder(self.ML)))
self.pub_enc_r.publish(Float64(data=self.g.get_motor_encoder(self.MR)))
self.pub_battery.publish(Float64(data=self.g.get_voltage_battery()))
# publish motor status, including encoder value
(flags, power, encoder, speed) = self.g.get_motor_status(self.ML)
status_left = MotorStatus(low_voltage=(flags & (1<<0)), overloaded=(flags & (1<<1)),
power=power, encoder=encoder, speed=speed)
(flags, power, encoder, speed) = self.g.get_motor_status(self.MR)
status_right = MotorStatus(low_voltage=(flags & (1<<0)), overloaded=(flags & (1<<1)),
power=power, encoder=encoder, speed=speed)
self.pub_motor_status.publish(MotorStatusLR(header=Header(stamp=rospy.Time.now()), left=status_left, right=status_right))
# publish current pose
(odom, transform)= self.odometry(status_left, status_right)
self.pub_odometry.publish(odom)
self.br.sendTransformMessage(transform)
rate.sleep()
self.g.reset_all()
# deactivate power management
os.write(self.gpio_value, "0".encode())
os.close(self.gpio_value)
# unexport pin
if os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN):
gpio_export = os.open("/sys/class/gpio/unexport", os.O_WRONLY)
os.write(gpio_export, self.POWER_PIN.encode())
os.close(gpio_export)
def close_hand():
msg = Float64()
msg.data = -1.0
for i in range(30):
pub.publish(msg)
rospy.sleep(0.1)
pub_cmd[10] = rospy.Publisher(pub_top[10], Float64)
pub_cmd[11] = rospy.Publisher(pub_top[11], Float64)
pub_cmd[12] = rospy.Publisher(pub_top[12], Float64)
pub_cmd[13] = rospy.Publisher(pub_top[13], Float64)
pub_cmd[14] = rospy.Publisher(pub_top[14], Float64)
pub_cmd[15] = rospy.Publisher(pub_top[15], Float64)
pub_cmd[16] = rospy.Publisher(pub_top[16], Float64)
pub_cmd[17] = rospy.Publisher(pub_top[17], Pr2GripperCommand)
pub_cmd[18] = rospy.Publisher(pub_top[18], Pr2GripperCommand)
pub_cmd[19] = rospy.Publisher(pub_top[19], JointTrajectory)
rospy.init_node(NAME, anonymous=False)
rospy.Subscriber(sub_top[0], JointControllerState, positionState)
cmd = Float64()
cmd.data = -0.3
timeout = 10.0
while True:
cmd.data = cmd.data * -1
start_time = time.time()
end_time = start_time + timeout
while time.time() < end_time:
pub_cmd[0].publish(cmd)
pub_cmd[1].publish(cmd)
pub_cmd[2].publish(cmd)
pub_cmd[3].publish(cmd)
pub_cmd[4].publish(cmd)
pub_cmd[5].publish(cmd)
def ReadMessages():
global carspeed
global rise_time
global settling_time
global overshoot
global ss_error
global e_prior
global es
global first
global bprev
global bpprev
global count
global firstst
global firstocl
global firstss
global cnt
global now
global setpoint1
global prev_time
global first_time
global set_po
global car_sp
global f
global print_paras
global control
global cmn_first
global stop_code
global reset_values
global count_log
try:
stsResult = PCAN_ERROR_OK
while (not (stsResult & PCAN_ERROR_QRCVEMPTY)):
result = m_objPCANBasic.Read(m_PcanHandle)
stsResult = result[0]
if result[0] == PCAN_ERROR_OK:
newMsg = result[1]
msgTimeStamp = result[2]
#msgTimeStamp.value = (msgTimeStamp.micros + 1000 * msgTimeStamp.millis + 0x100000000 * 1000 * msgTimeStamp.millis_overflow)
#print(format(newMsg.ID,'04x'))
MsgID = format(newMsg.ID, '04x')
DATA7 = format(newMsg.DATA[7], '02x')
DATA6 = format(newMsg.DATA[6], '02x')
if (MsgID == '076c'):
#print "Vehicle speed status"
print "\nSpeed of Car : ", int(DATA7, 16)
carspeed1 = int(DATA7, 16)
setpoint = Float64(setpoint1)
carspeed = Float64(carspeed1)
if (cmn_first == False):
e_prior = float(setpoint1) - float(carspeed1)
prev_time = rospy.get_time()
first_time = rospy.get_time()
car_sp.append([
float(carspeed1),
float(setpoint1), prev_time - first_time
])
#set_po.append(float(setpoint1))
pub2.publish(setpoint)
pub1.publish(carspeed)
elif (control == True):
now = rospy.get_time() - prev_time
prev_time = rospy.get_time()
e = float(setpoint1) - float(carspeed1)
ed = (e - e_prior) * now
es = (es + e * now)
# if((float(carspeed1)/float(setpoint1)*100>90) and first==True):
# rise_time=prev_time-first_time
# first=False
if (bprev - bpprev > 0 and carspeed1 - bprev < 0
and firstocl == True and bprev > setpoint):
print "max", bprev
#overshoot=(bprev-float(setpoint1))/float(setpoint1)*100
firstocl = False
if (ed < 0.0001):
count = count + 1
else:
count = 0
if (count > 100):
ss_error = e
if (bprev - bpprev < 0.001
and carspeed1 - bprev < 0.001
and firstst == True):
settling_time = prev_time - first_time
if (cnt > 0.01):
firstst = False
cnt = cnt + 1
bpprev = bprev
bprev = float(carspeed1)
e_prior = e
control = False
car_sp.append([
float(carspeed1),
float(setpoint1), prev_time - first_time
])
#set_po.append(float(setpoint1))
pub2.publish(setpoint)
pub1.publish(carspeed)
print "setpoint", setpoint
elif (print_paras == True):
print "rise_time", rise_time
print "settling_time", settling_time
print "overshoot", overshoot
print "steady_state_error", ss_error
print "current_carspeed", carspeed1
para_list = [
rise_time, settling_time, overshoot, ss_error,
float(carspeed1)
]
pickle.dump(car_sp, f)
pickle.dump(para_list, f)
l = open("full_pid_data1.txt", "a")
l.write("\n" + count_log)
l.write("\n" + str(car_sp))
count_log = count_log + 1
l.close()
print_paras = False
elif (reset_values == True):
control = False
rise_time = -100
settling_time = -100
overshoot = 0
ss_error = -100
e_prior = 0
es = 0
first = True
bprev = 0
bpprev = 0
count = 0
firstst = True
firstocl = True
firstss = True
cnt = 0
now = 0
prev_time = 0.0
first_time = 0.0
e2OCAN_RNDB.DATA[6] = RNDB_DRIVE
e2OCAN_ACCEL.DATA[7] = ACCEL_ZERO
e2OCAN_BRAKE.DATA[7] = BRAKE_ZERO
reset_values = False
#Logger= [carspeed , setpoint]
#f=open("test.txt","a")
#pickle.dump(Logger,f)
#elif (MsgID == '076e'):
#print "\n\nWiper & Horn Status"
#if(DATA7 == 'a0'):
#print "\nWiper : ON \nHorn : ON"
#elif(DATA7 == '20'):
#print "\nWiper : OFF \nHorn : ON"
#elif(DATA7 == '80'):
#print "\nWiper : ON \nHorn : OFF"
#elif(DATA7 == '00'):
#print "\nWiper : OFF \nHorn : OFF"
#elif (MsgID == '0771'):
#print "\n\nThrottle Status"
#Throttle = DATA7
#print "\nThrottel Percentage of Car : ", int(Throttle,16)/2
#elif (MsgID == '0773'):
#print "Braking Status"
#Brake = DATA7
#print "\nBraking Percentage of Car : ", int(Brake,16)/2
elif (MsgID == '0775'):
#print "\n\nSteering angle & Steering Direction Status"
Steer = DATA7
Steer_bin = int(Steer, 16)
#print Steer_bin
#if((Steer_bin&3)==2):
#print "\nSteering Direction : Clockwise"
#elif((Steer_bin&3)==1):
#print "\nSteering Direction : AntiClockwise"
#print "angle:",Steer_bin>>2
#print "\n Steering Angle : ",Steer_angle
#Steering Angle yet to be done
#elif (MsgID == '0777'):
#print "\n\nTurn Indicators & HL Beam"
#if(DATA7[0] == '8'):
#print "\nIndicator Status : Left Indicator"
#elif(DATA7[0] == '1'):
#print "\nIndicator Status : Right Indicator"
#elif(DATA7[0] == '9'):
#print "\nIndicator Status : Hazard Indicator"
#elif(DATA7[0] == '0'):
#print "\nIndicator Status : OFF"
#if(DATA7[1] == '4'):
#print "\nHead Lights : ON"
#elif(DATA7[1] == '0'):
#print "\nHead Lights : OFF"
#elif (MsgID == '0779'):
#print "\n\nDriving Mode & PRNDL Status"
#if(DATA7 == '40'):
#print "\nDriving Mode : Manual"
#elif(DATA7 == '80'):
#print "\nDriving Mode : Autonomous"
#elif(DATA7 == 'c0'):
#print "\nDriving Mode : Homing in Progress"
#else:
#print "\nDriving Mode : Invalid"
#if(DATA6 == '08'):
#print "\nGear Status : Reverse"
#elif(DATA6 == '10'):
#print "\nGear Status : Neutral"
#elif(DATA6 == '20'):
#print "\nGear Status : Drive"
#elif(DATA6 == '80'):
#print "\nGear Status : Boost"
#else:
#print "\nGear Status : Invalid"
#print(msgTimeStamp.value)
#for i in range(newMsg.LEN):
# print(format(newMsg.DATA[i],'02x'))
except KeyboardInterrupt:
tmrRead.stop()
m_objPCANBasic.Uninitialize(m_PcanHandle)
raise SystemExit, 1
def move(self):
self.pub.publish(Float64(self._goal_pos))
def __init__(self):
# ROS parameters
self.publish_frequency = rospy.get_param('~publish_frequency', 0.0)
frame_id = rospy.get_param('~frame_id', '')
child_frame_id = rospy.get_param('~child_frame_id', '')
display_diff = rospy.get_param('~display_diff', True)
display_RMSE = rospy.get_param('~display_RMSE', True)
# Initialise variables
self.odom1 = None
self.odom2 = None
sum_distance = 0.0 # sum of squared errors in position
sum_orientation = 0.0 # sum of squared errors in orientation
sum_linear_speed = 0.0 # sum of squared errors in linear speed
sum_angular_speed = 0.0 # sum of squared errors in angular speed
sum_nbr = 0.0 # nbr of errors in the sums
# ROS publishers
output_publisher = rospy.Publisher('diff_odom', Odometry, queue_size=1)
RMSE_distance_publisher = rospy.Publisher('RMSE_distance',
Float64,
queue_size=1)
RMSE_orientation_publisher = rospy.Publisher('RMSE_orientation',
Float64,
queue_size=1)
RMSE_orientation_publisher = rospy.Publisher('RMSE_linear_speed',
Float64,
queue_size=1)
RMSE_orientation_publisher = rospy.Publisher('RMSE_angular_speed',
Float64,
queue_size=1)
# ROS subscribers
rospy.Subscriber('odom1', Odometry, self.odom1_callback)
rospy.Subscriber('odom2', Odometry, self.odom2_callback)
# MAIN LOOP
rate = rospy.Rate(self.publish_frequency)
while not rospy.is_shutdown():
if (self.odom1 is not None) and (self.odom2 is not None):
# Compute and publish the difference between the two odometries
diff = Odometry()
diff.header.stamp = rospy.get_rostime()
diff.header.frame_id = frame_id
diff.child_frame_id = child_frame_id
diff.pose.pose.position.x = (self.odom1.pose.pose.position.x -
self.odom2.pose.pose.position.x)
diff.pose.pose.position.y = (self.odom1.pose.pose.position.y -
self.odom2.pose.pose.position.y)
diff_theta = self.compute_angle_difference(
self.odom1.pose.pose.orientation,
self.odom2.pose.pose.orientation)
quaternion = tf.transformations.quaternion_from_euler(
0, 0, diff_theta)
diff.pose.pose.orientation.x = quaternion[0]
diff.pose.pose.orientation.y = quaternion[1]
diff.pose.pose.orientation.z = quaternion[2]
diff.pose.pose.orientation.w = quaternion[3]
diff.twist.twist.linear.x = (self.odom1.twist.twist.linear.x -
self.odom2.twist.twist.linear.x)
diff.twist.twist.linear.y = (self.odom1.twist.twist.linear.y -
self.odom2.twist.twist.linear.y)
diff.twist.twist.angular.z = (
self.odom1.twist.twist.angular.z -
self.odom2.twist.twist.angular.z)
output_publisher.publish(diff)
# Compute and publish the RMSE
sum_distance += diff.pose.pose.position.x**2 + diff.pose.pose.position.y**2
sum_orientation += diff_theta**2
sum_linear_speed += diff.twist.twist.linear.x**2 + diff.twist.twist.linear.y**2
sum_angular_speed += diff.twist.twist.angular.z**2
sum_nbr += 1.0
RMSE_distance = sqrt(sum_distance / sum_nbr)
RMSE_orientation = sqrt(sum_orientation / sum_nbr)
RMSE_linear_speed = sqrt(sum_linear_speed / sum_nbr)
RMSE_angular_speed = sqrt(sum_angular_speed / sum_nbr)
RMSE_distance_publisher.publish(Float64(RMSE_distance))
RMSE_orientation_publisher.publish(Float64(RMSE_orientation))
RMSE_orientation_publisher.publish(Float64(RMSE_linear_speed))
RMSE_orientation_publisher.publish(Float64(RMSE_angular_speed))
if display_diff:
rospy.loginfo('Diff odom:')
rospy.loginfo(
"d_x={:.3f} ; d_y={:.3f} ; d_theta={:.3f}".format(
diff.pose.pose.position.x,
diff.pose.pose.position.y, diff_theta))
rospy.loginfo(
"d_v_x={:.3f} ; d_v_y={:.3f} ; d_w_z={:.3f}".format(
diff.twist.twist.linear.x,
diff.twist.twist.linear.y,
diff.twist.twist.angular.z,
))
if display_RMSE:
rospy.loginfo('RMSE diff odom:')
rospy.loginfo(
"d: {:.3f} ; theta: {:.3f} ; v: {:.3f} ; omega: {:.3f}"
.format(RMSE_distance, RMSE_orientation,
RMSE_linear_speed, RMSE_angular_speed))
rate.sleep()
