def addEllipsoid(self, ellipsoid_type='collision'):
if ellipsoid_type == 'collision':
id = 999
color = ColorRGBA(r=0, g=1, b=0, a=0.5)
cube = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.frame_id),
pose=self.collision_ellipsoid_origin,
type=Marker.SPHERE,
color=color,
id=id,
scale=Vector3(self.collision_ellipsoid_size_x,
self.collision_ellipsoid_size_y,
self.collision_ellipsoid_size_z),
action=Marker.ADD)
self.models[id] = cube
# self.deleteEllipsoid(ellipsoid_type='reaching')
elif ellipsoid_type == 'reaching':
id = 1000
color = ColorRGBA(r=1, g=0, b=1, a=1)
cube = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.frame_id),
pose=self.reaching_ellipsoid_origin,
type=Marker.SPHERE,
color=color,
id=id,
scale=Vector3(self.reaching_ellipsoid_size_x,
self.reaching_ellipsoid_size_y,
self.reaching_ellipsoid_size_z),
action=Marker.ADD)
self.models[id] = cube
# self.deleteEllipsoid(ellipsoid_type='collision')
elif ellipsoid_type == 'all':
id = 999
color = ColorRGBA(r=0, g=1, b=0, a=0.5)
cube = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.frame_id),
pose=self.collision_ellipsoid_origin,
type=Marker.SPHERE,
color=color,
id=id,
scale=Vector3(self.collision_ellipsoid_size_x,
self.collision_ellipsoid_size_y,
self.collision_ellipsoid_size_z),
action=Marker.ADD)
self.models[id] = cube
id = 1000
color = ColorRGBA(r=1, g=0, b=1, a=1)
cube = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.frame_id),
pose=self.reaching_ellipsoid_origin,
type=Marker.SPHERE,
color=color,
id=id,
scale=Vector3(self.reaching_ellipsoid_size_x,
self.reaching_ellipsoid_size_y,
self.reaching_ellipsoid_size_z),
action=Marker.ADD)
self.models[id] = cube
def update_dimensions(self, depth, width, height):
self.__scale = Vector3(width, depth, height)
def dropped_it(data):
if data.data == 1:
print "Hello we are in DROPPED_IT"
setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
setmodel(ModelState('object',Pose(Point(1.0, 0.0, 0.0005),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world'))
if __name__=="__main__":
rospy.init_node("q4")
topico_imagem = "/camera/rgb/image_raw/compressed"
velocidade_saida = rospy.Publisher("/cmd_vel", Twist, queue_size = 3 )
recebe_scan = rospy.Subscriber("/scan", LaserScan, scaneou)
recebedor = rospy.Subscriber(topico_imagem, CompressedImage, roda_todo_frame, queue_size=4, buff_size = 2**24)
delta_tolerance = 5
dist_tolerance = 0.1
metro = 1.0
rot = Twist(Vector3(0,0,0), Vector3(0,0,0.1))
frente = Twist(Vector3(0.2,0,0), Vector3(0,0,0))
zero = Twist(Vector3(0,0,0), Vector3(0,0,0))
PROCURANDO, CENTRALIZANDO, AVANCANDO, PARADO = 1,2,3,4
state = PROCURANDO
# Evitando bugs em algun setups
velocidade_saida.publish(zero)
rospy.sleep(1.0)
dt = 0.05
while not rospy.is_shutdown():
def run(self):
rospy.loginfo("ROS thread started.")
self.running = True
tf_pub = tf.TransformBroadcaster()
#rospy.Publisher('topic_name', std_msgs.msg.String, queue_size=10)
map_pub = rospy.Publisher('map', OccupancyGrid, queue_size=10)
odom_pub = rospy.Publisher("odom", Odometry, queue_size=10)
nav_goal_pub = rospy.Publisher("move_base_simple/goal",
PoseStamped,
queue_size=10)
rate = rospy.Rate(20) # 10hz
old_x = 0
old_y = 0
old_th = 0
last_time = rospy.Time.now()
global_frame = "map" # odom
child_frame = "base_link"
self.map_msg = OccupancyGrid()
self.map_msg.info.width = 640 # Map width
self.map_msg.info.height = 480 # Map height
self.map_msg.info.resolution = 0.005 # Map resolution
self.map_msg.header.frame_id = global_frame # New
odom_msg = Odometry()
odom_msg.header.frame_id = global_frame # odom
odom_msg.child_frame_id = child_frame
nav_goal_msg = PoseStamped()
nav_goal_msg.header.frame_id = global_frame
self.set_nav_goal = {"state": False}
while self.running and not rospy.is_shutdown():
#rospy.loginfo(msg)
'''***************************************
** Needs to be passed in the class
**transform_data = raw_map.get_transform()
***************************************'''
current_time = rospy.Time.now()
self.map_msg.header.stamp = current_time # New
self.map_msg.info.map_load_time = current_time # New
'''***************************************
**Needs to be passed in the class
**map_msg.data = raw_map.get_new_frame() #pass Frame here
***************************************'''
# Publishing the map
map_pub.publish(self.map_msg)
try:
if self.transform_data["state"] == 1:
#rospy.loginfo("New tf sent!")
# Getting the robot's position
x = self.transform_data["x"] * 0.005
y = self.transform_data["y"] * 0.005
th = self.transform_data["angle"]
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, th)
# Publshing the transforms
tf_pub.sendTransform(
(x, y, 0),
odom_quat,
current_time,
child_frame, # child frame
global_frame) # Parent frame odom
odom_msg.header.stamp = current_time
# Setting the pose of the robot
odom_msg.pose.pose.position.x = x
odom_msg.pose.pose.position.y = y
odom_msg.pose.pose.position.z = 0
odom_msg.pose.pose.orientation = Quaternion(*odom_quat)
# calculating velocities
dt = (current_time - last_time).to_sec()
vx = (x - old_x) / dt
vy = (y - old_y) / dt
vth = (th - old_th) / dt
# Setting the velocities
odom_msg.twist.twist.linear = Vector3(vx, vy, 0)
odom_msg.twist.twist.angular = Vector3(0, 0, vth)
# Publishing the odometery message
odom_pub.publish(odom_msg)
# Recording the values for the next loop Iteration
old_x = x
old_y = y
old_th = th
last_time = current_time
else:
#rospy.loginfo("lol tf failed")
'''
# Getting the robot's position
x = 1
y = 1
th = 0
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# Publshing the transforms
tf_pub.sendTransform((x, y, 0),
odom_quat,
current_time,
child_frame, # child frame
global_frame) # Parent frame odom
odom_msg.header.stamp = current_time
# Setting the pose of the robot
odom_msg.pose.pose.position.x = x
odom_msg.pose.pose.position.y = y
odom_msg.pose.pose.position.z = 0
odom_msg.pose.pose.orientation = Quaternion(*odom_quat)
# calculating velocities
dt = (current_time - last_time).to_sec()
vx = (x - old_x)/dt
vy = (y - old_y)/ dt
vth = (th - old_th)/ dt
# Setting the velocities
odom_msg.twist.twist.linear = Vector3(vx, vy, 0)
odom_msg.twist.twist.angular = Vector3(0, 0, vth)
# Publishing the odometery message
odom_pub.publish(odom_msg)
'''
except Exception as e:
rospy.loginfo("Was about to die, check error below:")
rospy.loginfo(e)
if self.set_nav_goal["state"]:
nav_goal_msg.header.stamp = rospy.Time.now()
nav_goal_msg.pose.pose.position.x = self.set_nav_goal["x"]
nav_goal_msg.pose.pose.position.y = self.set_nav_goal["y"]
nav_goal_odom_quat = tf.transformations.quaternion_from_euler(
0, 0, self.set_nav_goal["th"])
nav_goal_msg.pose.pose.orientation = Quaternion(
*nav_goal_odom_quat)
nav_goal_pub.publish(nav_goal_msg)
#rospy.loginfo("In the thread!!")
rate.sleep()
rospy.loginfo("ROS thread stopped")
def visualizer():
rospy.Subscriber("baro/pressure", FluidPressure, update_pressure)
rospy.Subscriber("gps", GPS, update_gps)
rate = rospy.Rate(1)
trace_pub = rospy.Publisher("viz/gps_trace", Marker)
primary_geofence_pub = rospy.Publisher("viz/primary_geofence", Marker)
secondary_geofence_pub = rospy.Publisher("viz/secondary_geofence", Marker)
trace_points = []
# initialize parameters
secondary_geofence_width = rospy.get_param("secondary_geofence/width",
0.00002)
min_lon = 0
max_lon = 0
min_lat = 0
max_lat = 0
try:
min_lon = rospy.get_param("geofence/lon/min")
max_lon = rospy.get_param("geofence/lon/max")
min_lat = rospy.get_param("geofence/lat/max")
max_lat = rospy.get_param("geofence/lat/min")
except KeyError:
rospy.logfatal(
"Geofence was not configured on the parameter server! Shutting down"
)
sys.exit(1)
# get parameters with defaults that require the aforementioned non-default parameters
secondary_min_lat = rospy.get_param("secondary_geofence/lat/min",
min_lat + secondary_geofence_width)
secondary_max_lat = rospy.get_param("secondary_geofence/lat/max",
max_lat - secondary_geofence_width)
secondary_min_lon = rospy.get_param("secondary_geofence/lon/min",
min_lon - secondary_geofence_width)
secondary_max_lon = rospy.get_param("secondary_geofence/lon/max",
max_lon + secondary_geofence_width)
while not rospy.is_shutdown():
if fix and pressure_offset != -1:
gps_divisor = rospy.get_param("~gps_divisor", 0.00001)
try:
primary_geofence_pts = (Point(
0, 0, 0), Point(0, (max_lat - min_lat) / gps_divisor, 0),
Point(
(max_lon - min_lon) / gps_divisor,
(max_lat - min_lat) / gps_divisor,
0),
Point(
(max_lon - min_lon) / gps_divisor,
0, 0), Point(0, 0, 0))
secondary_geofence_pts = (
Point((secondary_min_lon - min_lon) / gps_divisor,
(secondary_min_lat - min_lat) / gps_divisor, 0),
Point((secondary_min_lon - min_lon) / gps_divisor,
(secondary_max_lat - min_lat) / gps_divisor, 0),
Point((secondary_max_lon - min_lon) / gps_divisor,
(secondary_max_lat - min_lat) / gps_divisor, 0),
Point((secondary_max_lon - min_lon) / gps_divisor,
(secondary_min_lat - min_lat) / gps_divisor, 0),
Point((secondary_min_lon - min_lon) / gps_divisor,
(secondary_min_lat - min_lat) / gps_divisor, 0))
trace_points.append(
Point((lon - min_lon) / gps_divisor,
(lat - min_lat) / gps_divisor,
relative_alt / rospy.get_param("~baro_divisor", 10)))
trace_color = rospy.get_param("~trace_color", {
"r": 0,
"g": 1,
"b": 0,
"a": 1
})
viz_trace_color = ColorRGBA(trace_color['r'], trace_color['g'],
trace_color['b'], trace_color['a'])
primary_geofence_color = rospy.get_param(
"~primary_geofence_color", {
"r": 1,
"g": 1,
"b": 0,
"a": 1
})
viz_primary_geofence_color = ColorRGBA(
primary_geofence_color['r'], primary_geofence_color['g'],
primary_geofence_color['b'], primary_geofence_color['a'])
secondary_geofence_color = rospy.get_param(
"~secondary_geofence_color", {
"r": 1,
"g": 0,
"b": 0,
"a": 1
})
viz_secondary_geofence_color = ColorRGBA(
secondary_geofence_color['r'],
secondary_geofence_color['g'],
secondary_geofence_color['b'],
secondary_geofence_color['a'])
scale = rospy.get_param("~scale", 0.25)
scale_vec = Vector3(scale, 0, 0)
trace_ns = rospy.get_param("~trace_ns", "moby_trace")
primary_geofence_ns = rospy.get_param("~primary_geofence_ns",
"moby_primary_geofence")
secondary_geofence_ns = rospy.get_param(
"~secondary_geofence_ns", "moby_secondary_geofence")
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "map"
trace_msg = Marker(header=header,
ns=trace_ns,
id=0,
type=4,
action=0,
scale=scale_vec,
color=viz_trace_color,
lifetime=rospy.Duration(),
frame_locked=False,
points=trace_points)
trace_msg.pose.orientation.w = 1.0
trace_pub.publish(trace_msg)
primary_geofence_msg = Marker(header=header,
ns=primary_geofence_ns,
id=0,
type=4,
action=0,
scale=scale_vec,
color=viz_primary_geofence_color,
lifetime=rospy.Duration(),
frame_locked=False,
points=primary_geofence_pts)
primary_geofence_pub.publish(primary_geofence_msg)
secondary_geofence_msg = Marker(
header=header,
ns=secondary_geofence_ns,
id=0,
type=4,
action=0,
scale=scale_vec,
color=viz_secondary_geofence_color,
lifetime=rospy.Duration(),
frame_locked=False,
points=secondary_geofence_pts)
secondary_geofence_pub.publish(secondary_geofence_msg)
except KeyError:
rospy.logerr(
"Visualization is enabled but zero point(s) are not set!")
rate.sleep()
def calibrate_tool():
rospy.init_node('calibrate_tool')
tool_name = rospy.get_param('~tool_name')
robot_name = rospy.get_param('~robot_name')
store_to_file = rospy.get_param('~store_to_file')
robot = rospy.get_param('~robot')
if robot == "kuka":
joint_names = rospy.get_param('/controller_joint_names')
controller_topic = '/position_trajectory_controller/command'
calcOffset_service = '/CalculateAverageMasurement'
else:
joint_names = rospy.get_param('/arm/joint_names')
controller_topic = '/arm/joint_trajectory_controller/command'
calcOffset_service = '/arm/CalculateAverageMasurement'
trajectory_pub = rospy.Publisher(controller_topic, JointTrajectory, latch=True, queue_size=1)
average_measurements_srv = rospy.ServiceProxy(calcOffset_service, CalculateAverageMasurement)
# Posees
poses = [[0.0, 0.0, 1.5707963, 0.0, -1.5707963, 0.0],
[0.0, 0.0, 1.5707963, 0.0, 1.5707963, 0.0],
[0.0, 0.0, 0.0, 0.0, 1.5707963, 0.0]]
poses_kuka = [[0.0, -1.5707963, 1.5707963, 0.0, -1.5707963, 0.0],
[0.0, -1.5707963, 1.5707963, 0.0, 1.5707963, 0.0],
[0.0, -1.5707963, 1.5707963, 0.0, 0.0, 0.0]]
measurement = []
for i in range(0,len(poses)):
trajectory = JointTrajectory()
point = JointTrajectoryPoint()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = joint_names
point.time_from_start = rospy.Duration(5.0)
if robot == "kuka":
point.positions = poses_kuka[i]
else:
point.positions = poses[i]
trajectory.points.append(point)
trajectory_pub.publish(trajectory)
rospy.loginfo("Going to position: " + str(point.positions))
rospy.sleep(10.0)
rospy.loginfo("Calculating tool force.")
#ret = average_measurements_srv(500, 0.01)
ret = average_measurements_srv(500, 0.01, "fts_base_link")
measurement.append(ret.measurement)
CoG = Vector3()
Fg = (abs(measurement[0].force.z) + abs(measurement[1].force.z))/2.0;
CoG.z = (sqrt(measurement[2].torque.x*measurement[2].torque.x + measurement[2].torque.y*measurement[2].torque.y)) / Fg;
#CoG.z = (measurement[2].torque.x) / Fg;
rospy.loginfo("Setting parametes for tool: " + tool_name)
rospy.set_param('/temp/tool/CoG_x', CoG.x)
rospy.set_param('/temp/tool/CoG_y', CoG.y)
rospy.set_param('/temp/tool/CoG_z', CoG.z)
rospy.set_param('/temp/tool/force', Fg)
if store_to_file:
call("rosparam dump -v `rospack find iirob_description`/tools/urdf/" + tool_name + "/" + robot_name + "_gravity.yaml /temp/tool", shell=True)
def publish_desired_acc(self):
pub_vec3 = Vector3Stamped()
pub_vec3.header.stamp = rospy.Time.now()
pub_vec3.vector = Vector3(self.desired_acceleratons[0, 0], self.desired_acceleratons[1, 0], \
self.desired_acceleratons[2, 0])
self.pub_desired_acc.publish(pub_vec3)
def sim_odom_callback(odom_msg):
odom_msg.pose.pose.orientation = Quaternion()
odom_msg.pose.pose.orientation.w = 1.0
odom_msg.twist.twist.angular = Vector3()
odom_msg.child_frame_id = 'level_quad'
odom_pub.publish(odom_msg)
def main():
rospy.init_node('simple_marker')
wait_for_time()
global pub_init_pose
pub_init_pose = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=1)
pub = rospy.Publisher('odometry_goal', Point, queue_size=5)
hostname = socket.gethostname()
marker_publisher = rospy.Publisher('visualization_marker',
Marker,
queue_size=5)
marker_publisher_host = rospy.Publisher(hostname + '/visualization_marker',
Marker,
queue_size=5)
waypoints_publisher = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=5)
waypoints_publisher_text = rospy.Publisher(
'visualization_marker_array_text', MarkerArray, queue_size=5)
rospy.sleep(0.5)
server = InteractiveMarkerServer('simple_marker')
marker1 = DestinationMarker(server, 0, 0, 'dest1', pub)
global pose_marker
pose_marker = DestinationMarker(server, 1, 0, 'pose estimate', pub)
global waypoint_marker
waypoint_marker = DestinationMarker(server, 2, -1, 'waypoint', pub)
marker1.start()
marker1.markerOff()
pose_marker.start()
waypoint_marker.start()
global waypoint_list
#marker1.markerOff()
waypoint_marker.markerOff()
pose_marker.markerOff()
rospy.Subscriber("robot_pose", PoseStamped, robot_marker_callback)
service_waypoint = rospy.Service('waypoint_markers_service', waypoint,
turn_waypoint_on_off)
service_pose_estimate = rospy.Service('poseestimate_markers_service',
poseestimate,
turn_poseestimate_on_off)
service_start_slam = rospy.Service('start_slam', slamsrv, turn_slam_on_off)
service_start_nav = rospy.Service('start_navigation', navigatesrv,
turn_nav_on_off)
set_pose = rospy.Service('set_pose', poseestimate, set_post)
get_map = rospy.Service('get_map', maps, get_maps)
getpose = rospy.Service('get_pose', getpost, get_pose)
get_map = rospy.Service('save_map', savemaps, save_maps)
get_waypoint = rospy.Service('get_waypoint', waypointsarray, getwaypoints)
get_a_waypoint = rospy.Service('get_a_waypoint', awaypoint,
getwayAwaypoint)
get_waypoint_name = rospy.Service('get_waypoint_name', waypointname,
getWaypointname)
delete_map = rospy.Service('delete_map', deletemap, deleteMap)
delete_waypoint = rospy.Service('delete_waypoint', deletewaypoint,
deleteWaypoint)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
rate = rospy.Rate(20.0)
is_slam_running = False
is_nav_running = False
wpts = []
p_wpts = []
markerArray2 = MarkerArray()
markerArray3 = MarkerArray()
p_waypoint_length = 0
print("Start loop")
while not rospy.is_shutdown():
if not q_slam.empty():
status = q_slam.get()
if status == 1:
if is_slam_running == False:
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch_slam = roslaunch.parent.ROSLaunchParent(
uuid, [
"/home/pi/linorobot_ws/src/linorobot/launch/slam.launch"
])
launch_slam.start()
is_slam_running = True
print("start")
else:
if is_slam_running == True:
launch_slam.shutdown()
is_slam_running = False
print("stop")
if not q_nav.empty():
nav_status = q_nav.get()
if nav_status["sw"] == 1:
if is_slam_running == False:
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
hostname = socket.gethostname()
cli_args = [
"/home/pi/linorobot_ws/src/linorobot/launch/navigate.launch",
"map_file:=/home/pi/linorobot_ws/src/linorobot/maps/" +
nav_status["mapname"] + ".yaml"
]
roslaunch_args = cli_args[1:]
roslaunch_file = [(
roslaunch.rlutil.resolve_launch_arguments(cli_args)[0],
roslaunch_args)]
launch_nav = roslaunch.parent.ROSLaunchParent(
uuid, roslaunch_file) #map_file
launch_nav.start()
print(hostname)
#uuid2 = roslaunch.rlutil.get_or_generate_uuid(None, False)
#roslaunch.configure_logging(uuid2)
#hostname = socket.gethostname()
#cli_args2 = ["/home/pi/linorobot_ws/src/mqtt_bridge/launch/demo.launch"]
#roslaunch_args2 = cli_args2[1:]
#roslaunch_file2 = [(roslaunch.rlutil.resolve_launch_arguments(cli_args2)[0], roslaunch_args2)]
#launch_nav2 = roslaunch.parent.ROSLaunchParent(uuid2, roslaunch_file2) #map_file
is_nav_running = True
print("start")
else:
if is_nav_running == True:
launch_nav.shutdown()
#launch_nav2.shutdown()
is_nav_running = False
print("stop")
if not q_map_name.empty():
mapname = q_map_name.get()
bin_cmd = 'rosrun map_server map_saver -f /home/pi/linorobot_ws/src/linorobot/maps/' + mapname
print bin_cmd
os.system(bin_cmd)
data = {}
with open(os.path.join(MAP_PATH, mapname + '.json'), 'w') as fp:
json.dump(data, fp)
#subprocess.run(["rosrun", bin_cmd])
if not q_wayponts.empty():
wpts = q_wayponts.get()
wp_names = q_wayponts_names.get()
print "========>"
print len(wpts)
#print wpts
mk_length = len(markerArray2.markers)
id = 0
id_t = len(markerArray2.markers) + 1
i = 0
del markerArray2.markers[:]
del markerArray3.markers[:]
for x in range(0, mk_length):
#marker =
markerArray2.markers.append(
Marker(type=Marker.ARROW,
id=id,
action=2,
scale=Vector3(0.2, 0.2, 0.2),
header=Header(frame_id='map'),
color=ColorRGBA(1.0, 1.0, 0.0, 1.0),
text="AGV"))
markerArray3.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=id_t,
action=2,
scale=Vector3(0.3, 0.3, 0.3),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 0.1, 1.0, 1.0),
text="null"))
i = i + 1
id += 1
id_t += 1
waypoints_publisher.publish(markerArray2)
waypoints_publisher_text.publish(markerArray3)
print "clear========>"
del markerArray2.markers[:]
del markerArray3.markers[:]
#markerArray2.markers = []
#markerArray3.markers = []
id = 0
id_t = len(wpts) + 1
i = 0
for x in wpts:
#marker =
markerArray2.markers.append(
Marker(type=Marker.ARROW,
id=id,
pose=x,
lifetime=rospy.Duration.from_sec(1),
scale=Vector3(0.2, 0.2, 0.2),
header=Header(frame_id='map'),
color=ColorRGBA(1.0, 1.0, 0.0, 1.0),
text="AGV"))
ik_pose = Pose()
ik_pose.position.x = x.position.x
ik_pose.position.y = x.position.y
ik_pose.position.z = x.position.z + 0.1
ik_pose.orientation.x = x.orientation.x
ik_pose.orientation.y = x.orientation.y
ik_pose.orientation.z = x.orientation.z
ik_pose.orientation.w = x.orientation.w
markerArray3.markers.append(
Marker(type=Marker.TEXT_VIEW_FACING,
id=id_t,
pose=ik_pose,
scale=Vector3(0.3, 0.3, 0.3),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 0.1, 1.0, 1.0),
text=wp_names[i]))
i = i + 1
id += 1
id_t += 1
p_waypoint_length = len(wpts)
p_wpts = wpts
#print markerArray3
waypoints_publisher.publish(markerArray2)
waypoints_publisher_text.publish(markerArray3)
try:
point_odom = tfBuffer.lookup_transform('map', 'base_footprint',
rospy.Time(0))
hhname = socket.gethostname()
marker = Marker(type=Marker.ARROW,
id=0,
lifetime=rospy.Duration(1.5),
pose=Pose(point_odom.transform.translation,
point_odom.transform.rotation),
scale=Vector3(0.2, 0.2, 0.2),
header=Header(frame_id='map'),
color=ColorRGBA(0.0, 1.0, 0.0, 1.0),
text=hhname)
marker_publisher.publish(marker)
marker_publisher_host.publish(marker)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
#continue
#print("loop")
rate.sleep()
def update(self, timestamp, pose, angular_velocity, linear_acceleration):
# Prepare state vector (pose only; ignore angular_velocity, linear_acceleration)
state = np.array([
pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]).reshape(1, -1)
# Compute reward / penalty and check if this episode is complete
done = False
if (timestamp < 2 and pose.position.z < self.target_z):
reward = -min(
abs(self.target_z - pose.position.z), 20.0
) # reward = zero for matching target z, -ve as you go farther, upto -20
if pose.position.z >= self.target_z: # agent has crossed the target height
reward += 10.0 # bonus reward
# takeoff_done = True
# Hover reward
elif (timestamp < 4):
reward = -min(
abs(self.target_z - pose.position.z), 20.0
) # reward = zero for matching target z, -ve as you go farther, upto -20
if abs(pose.position.z - self.target_z
) >= 2: # agent has gone deviated from the target height
reward -= 10.0 # bonus reward
# Landing reward
elif (timestamp < 6):
reward = (
abs(pose.position.z) * 3 + abs(angular_velocity.x) +
abs(angular_velocity.y) + abs(angular_velocity.z) +
abs(linear_acceleration.x) + abs(linear_acceleration.y) +
abs(linear_acceleration.z)
) # reward = zero for matching target z, -ve as you go farther, upto -20
# Normalizing the reward
reward = -min(reward / 3, 20.0)
if pose.position.z <= 0.8: # agent has crossed the target height
reward += 10.0 # bonus reward
elif timestamp > self.max_duration: # or pose.position.x > 5 or pose.position.x < -5 or pose.position.y > 5 or pose.position.y < -5: # agent has run out of time
reward -= 10.0 # extra penalty
done = True
else:
done = True
reward = -min(
abs(self.target_z - pose.position.z), 20.0
) # reward = zero for matching target z, -ve as you go farther, upto -20
if pose.position.z >= self.target_z: # agent has crossed the target height
reward += 10.0 # bonus reward
done = True
elif timestamp > self.max_duration: # agent has run out of time
reward -= 10.0 # extra penalty
done = True
# Take one RL step, passing in current state and reward, and obtain action
# Note: The reward passed in here is the result of past action(s)
action = self.agent.step(state, reward,
done) # note: action = <force; torque> vector
# Convert to proper force command (a Wrench object) and return it
if action is not None:
action = np.clip(action.flatten(), self.action_space.low,
self.action_space.high
) # flatten, clamp to action space limits
return Wrench(
force=Vector3(action[0], action[1], action[2]),
torque=Vector3(action[3], action[4], action[5])
# force=Vector3(0,0, action[2]),
# torque=Vector3(0,0,0)
), done
else:
return Wrench(), done
rospy.Subscriber("ardu_send_imu", Imu, sub_imu_nf)
rospy.Subscriber("ardu_send_mag", MagneticField, sub_mag_nf)
if calibration == 'Previous':
offset = get_previous_imu()
elif calibration == 'New':
offset = calibrate_imu()
else:
offset = np.zeros((9, 1))
rospy.loginfo("\nCalibration done")
raw_imu = vect_imu + offset
pub_send_euler_angles = rospy.Publisher('filter_send_euler_angles',
Vector3,
queue_size=10)
euler_angles_msg = Vector3()
P0 = 10 * np.eye(3)
Q = 0.028**2 * np.eye(3) #0.028
R = 0.01 * np.eye(3)
EKF_yaw = Extended_kalman_filter(np.zeros((3, 1)), P0, f, F, h, H, Q, R)
EKF_pitch = Extended_kalman_filter(np.zeros((3, 1)), P0, f, F, h, H, Q, R)
EKF_roll = Extended_kalman_filter(np.zeros((3, 1)), P0, f, F, h, H, Q, R)
q = Quaternion(1, 0, 0, 0)
alpha = 0.2
low_pass_imu = raw_imu.copy()
rospy.loginfo("\nStart main loop")
while not rospy.is_shutdown():
m.pose.orientation.w = 1
m.scale = scale
m.color.r = 0.2
m.color.g = 0.5
m.color.b = 1.0
m.color.a = 0.3
m.points = [tail, tip]
return m
while not rospy.is_shutdown():
rospy.loginfo("Publishing arrow marker")
# marker_pub.publish(make_arrow_points_marker(Point(0,0,0), Point(2,2,0), 0))
# marker_pub.publish(make_arrow_points_marker(Point(0,0,0), Point(1,-1,1), 1))
# marker_pub.publish(make_arrow_points_marker(Point(-1,-1,-1), Point(1,-1,-1), 2))
# this arrow should look exactly like the other one, except that is
# is twice a wide in the z direction.
scale = Vector3(2, 4, 0.69)
marker_pub.publish(
make_arrow_points_marker(scale, Point(0, 0, 0), Point(3, 0, 0), 3))
scale = Vector3(3, 2, 1)
marker_pub.publish(make_marker(Marker.SPHERE, scale, 1, 0.5, 0.2, 0.3))
marker_pub.publish(make_marker(Marker.CYLINDER, scale, 0.5, 0.2, 1, 0.3))
marker_pub.publish(make_marker(Marker.CUBE, scale, 0.2, 1, 0.5, 0.3))
marker_pub.publish(make_marker(Marker.ARROW, scale, 1, 1, 1, 0.5))
rospy.sleep(1.0)
def listerner_callback(self, msg):
#instiate needed varibles and constants
cv_bridge = CvBridge()
img = cv_bridge.imgmsg_to_cv2(msg)
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
angle = 0.365
max_height = 350
min_height = 200
#cut off top and bottom of image
img = img[len(img) - max_height:len(img) - min_height]
#change the resolution of the image for easier computing
img = cv2.resize(img, (100, int((max_height - min_height) / 2)),
interpolation=cv2.INTER_AREA)
#determine the width of the image
img_width = len(img[0])
#create a second image to copy the first image into
img2 = np.zeros((len(img), len(img[0])))
#cuts off portions of the image so teh resutling image is of the projection of a rectangle on the ground projected onto the camera
for i in range(len(img)):
#determine left and right cutoff for that row
left_cutoff = int(i * angle)
right_cutoff = len(img[i]) - (left_cutoff)
temp = np.zeros((1, right_cutoff - left_cutoff))
#cutoff the sides and set hte img row to the resulting row with cutoffs
temp[0] = img[len(img) - 1 - i][left_cutoff:right_cutoff]
temp = cv2.resize(temp, (img_width, 1),
interpolation=cv2.INTER_AREA)
img[len(img) - 1 - i] = temp[0]
#if first image, set the scanline as the reference scanline
if (self.first):
self.reference_scanline = self.scanLine(img)
self.first = False
#set some constants
min_radius = 200
searches = 50
curr = -1 / min_radius
max_added_width = int(
math.ceil(math.sqrt(min_radius**2 - ((len(img) - 1)**2))))
max_width = img_width + 2 * max_added_width
values = np.zeros(searches)
#preforms the shifts on the image based on guessed turn radius
for i in range(searches):
temp_image = np.zeros((len(img), max_width))
if (curr == 0):
shift = 0
values[i] = self.score(img)
continue
rad = 1 / curr
#prefroms shift
for j in range(len(img)):
shift = int(abs(rad) - math.sqrt(rad**2 - j**2))
if (rad < 0):
shift *= -1
temp_image[len(img) - 1 -
j][max_added_width + shift:max_added_width + shift +
img_width] = img[len(img) - 1 - j]
values[i] = self.score(temp_image)
curr += (1 / min_radius) / (searches / 2)
turn_radius = 0
best = 0
#finds best fitting raidus best on values array
for i in range(searches):
if (values[i] > best):
best = values[i]
turn_radius = (-1 / min_radius) + ((1 / min_radius) /
(searches / 2)) * i
#recreates the best shifted image for debugging purposes
shifted_best = np.zeros((len(img), max_width))
if (turn_radius == 0):
shifted_best = img
else:
rad = 1 / turn_radius
for j in range(len(img)):
shift = int(abs(rad) - math.sqrt(rad**2 - j**2))
if (rad < 0):
shift *= -1
shifted_best[len(img) - 1 -
j][max_added_width + shift:max_added_width +
shift + img_width] = img[len(img) - 1 - j]
#calculates the offset
offset = self.offset_calc(self.scanLine(shifted_best))
#print the calculated offset and turn radius
self.get_logger().info('offset: "%f"' % offset)
self.get_logger().info('turn_raidus: "%f"' % turn_radius)
#calculates the robot's speed and rotation
twist_msg = Twist()
twist_msg.angular = Vector3()
x_speed = 0.5
offset_r = offset * .03
turn_radius_r = turn_radius / 0.03
#applies maximium value to the rotation caused by offset
if (offset_r > 0):
offset_r = min(offset_r, 0.3)
else:
offset_r = max(offset_r, -0.3)
#applies maximium value to the rotation cuase by turn_radius
if (turn_radius_r > 0):
turn_radius_r = min(turn_radius_r, 0.8)
else:
turn_radius_r = max(turn_radius_r, -0.8)
#slows down the robot and lowers offset rotation influence if the turn is too tight
if (abs(turn_radius) > 0.004):
x_speed -= abs(turn_radius - 0.004) * 5
if (offset_r * turn_radius_r < 0):
if (offset_r > 0):
offset_r = min(offset_r, 0.15)
else:
offset_r = max(offset_r, -0.15)
#set the twist object to the robots speed and rotation
twist_msg.angular = Vector3()
twist_msg.angular.z = turn_radius_r + offset_r
self.get_logger().info('turning: "%f"' % (turn_radius_r + offset_r))
self.get_logger().info('turning: "%f"' % twist_msg.angular.z)
twist_msg.linear = Vector3()
twist_msg.linear.x = x_speed
#publish the twist object with teh robots speed and rotation
self.twist_pub.publish(twist_msg)
def publish(self):
euler_angles_msg = Vector3()
euler_angles_msg.x = self.yaw
euler_angles_msg.y = self.pitch
euler_angles_msg.z = self.roll
self.pub_send_euler_angles.publish(euler_angles_msg)
def publish_desired_vel(self):
pub_vec3 = Vector3Stamped()
pub_vec3.header.stamp = rospy.Time.now()
pub_vec3.vector = Vector3(self.desired_velocities[0, 0], self.desired_velocities[1, 0], \
self.desired_velocities[2, 0])
self.pub_desired_vel.publish(pub_vec3)
HEIGHT = 500
pygame.display.set_caption('Drone Control')
windowSurface = pygame.display.set_mode((WIDTH, HEIGHT), 0, 32)
windowSurface.fill(BLACK)
font = pygame.font.Font(pygame.font.get_default_font(), 10)
myText = 't: Takeoff, l: Land, w: Forward, a: Left, s: Backward, d: Right, q: Up, e: Down, z: RotateLeft, x: RotateRight'
myTextSurface = font.render(myText, True, WHITE)
textRect = myTextSurface.get_rect()
textRect.center = (WIDTH // 2, HEIGHT // 2)
windowSurface.blit(myTextSurface, textRect)
pygame.display.update()
while True:
setVelocity.twist.linear = Vector3(x=0, y=0, z=0)
setVelocity.twist.angular = Vector3(x=0, y=0, z=0)
for event in pygame.event.get():
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_t:
setMode()
armCopter()
takeOff()
time.sleep(5)
if event.key == pygame.K_l:
land()
disarmCopter()
time.sleep(5)
if event.key == pygame.K_w:
# move forward
def publish_desired_snap(self):
pub_vec3 = Vector3Stamped()
pub_vec3.header.stamp = rospy.Time.now()
pub_vec3.vector = Vector3(self.desired_snap[0, 0], self.desired_snap[1, 0], \
self.desired_snap[2, 0])
self.pub_desired_snap.publish(pub_vec3)
def stopMoving(self):
velocity = Twist()
velocity.linear = Vector3(0., 0., 0.)
velocity.angular = Vector3(0., 0., 0.)
self.cmdVelPublisher.publish(velocity)
##########################################
# Joint trayectory control UR5e Geomagic #
##########################################
############
# Var init #
############
#################
# Main function #
#################
# Inicializo nodo - haptic_jointpose -
rospy.init_node('testing', anonymous=False)
r = rospy.Rate(10)
pub = rospy.Publisher('/Geomagic/force_feedback', DeviceFeedback, queue_size=1)
df = DeviceFeedback()
df.force = Vector3()
df.lock = [False]
while not rospy.is_shutdown():
df.force.x = 0
df.force.y = 0
df.force.z = 0
pub.publish(df)
r.sleep()
def callback(self, data):
drone01 = data.pose[idx[0]]
drone01_vel = data.twist[idx[0]]
d01_posX = drone01.position.x
d01_posY = drone01.position.y
d01_posZ = drone01.position.z
d01_velX = drone01_vel.linear.x
d01_velY = drone01_vel.linear.y
drone02 = data.pose[idx[1]]
drone02_vel = data.twist[idx[1]]
d02_posX = drone02.position.x
d02_posY = drone02.position.y
d02_posZ = drone02.position.z
d02_velX = drone02_vel.linear.x
d02_velY = drone02_vel.linear.y
drone04 = data.pose[idx[3]]
drone04_vel = data.twist[idx[3]]
d04_posX = drone04.position.x
d04_posY = drone04.position.y
d04_posZ = drone04.position.z
d04_velX = drone04_vel.linear.x
d04_velY = drone04_vel.linear.y
drone05 = data.pose[idx[4]]
drone05_vel = data.twist[idx[4]]
d05_posX = drone05.position.x
d05_posY = drone05.position.y
d05_posZ = drone05.position.z
d05_velX = drone05_vel.linear.x
d05_velY = drone05_vel.linear.y
drone06 = data.pose[idx[5]]
drone06_vel = data.twist[idx[5]]
d06_posX = drone06.position.x
d06_posY = drone06.position.y
d06_posZ = drone06.position.z
d06_velX = drone06_vel.linear.x
d06_velY = drone06_vel.linear.y
drone07 = data.pose[idx[6]]
drone07_vel = data.twist[idx[6]]
d07_posX = drone07.position.x
d07_posY = drone07.position.y
d07_posZ = drone07.position.z
d07_velX = drone07_vel.linear.x
d07_velY = drone07_vel.linear.y
drone08 = data.pose[idx[7]]
drone08_vel = data.twist[idx[7]]
d08_posX = drone08.position.x
d08_posY = drone08.position.y
d08_posZ = drone08.position.z
d08_velX = drone08_vel.linear.x
d08_velY = drone08_vel.linear.y
#print(control,d04_posZ)
####################
## POSITION BASED ##
####################
velX, velY, velZ = control
#print("ini velx: ", velX)
repul = 0.0
pub = self.cmd_vel_pub
curr_time = time.time() - start
dist_87 = float(
math.sqrt((d08_posX - d07_posX)**2 + (d08_posY - d07_posY)**2))
dist_76 = float(
math.sqrt((d06_posX - d07_posX)**2 + (d06_posY - d07_posY)**2))
if d07_posZ < 2 and dist_87 > 15:
calc_odom = (Vector3(-0.1 * (d07_velX - d08_posX),
-0.1 * (d07_posY - d08_posY),
-0.05 * (d07_posZ - 3)))
print("ATR APPEAR!!")
else:
if d07_posZ >= 2 and curr_time > 10:
if dist_87 <= MAX_DIST and d08_posZ >= 2:
#print("d87 REPUL APPEAR!!", dist_87)
b87 = 1.8 * float((20.0 / (MAX_DIST**7)) * (dist_87**7) -
(70.0 / (MAX_DIST**6)) * (dist_87**6) +
(84.0 / (MAX_DIST**5)) * (dist_87**5) -
(35.0 /
(MAX_DIST**4)) * (dist_87**4) + 1)
else:
b87 = 0.0
if dist_76 <= MAX_DIST and d06_posZ >= 2:
#print("d76 REPUL APPEAR!!", dist_76)
b76 = -0.4 * float((20.0 / (MAX_DIST**7)) * (dist_76**7) -
(70.0 / (MAX_DIST**6)) * (dist_76**6) +
(84.0 / (MAX_DIST**5)) * (dist_76**5) -
(35.0 /
(MAX_DIST**4)) * (dist_76**4) + 1)
else:
b76 = 0.0
repul = round((b76 + b87), 2)
else:
repul = 0.0
new_velX = velX - repul
calc_odom = (Vector3(new_velX, velY, -0.8 * (d07_posZ - 3)))
pub.publish(Twist(calc_odom, Vector3(0, 0, 0)))
#print('position control now')
rospy.Rate(1).sleep
def main():
rospy.init_node('object', anonymous=True)
#Initial location (reset)
setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
setmodel(ModelState('object',Pose(Point(0.0,0.0,0.0005),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world'))
#Run loop for all the simulation time length
while not rospy.is_shutdown():
rospy.sleep(0.01)
rospy.Subscriber('robot_has_piece', Int8,correct_gazebo)
if correction == 0:
getmodel = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
data = getmodel('object','')
piece=rospy.Publisher('piece_location', Point,queue_size=1,latch=True)
piece.publish(data.pose.position.x+0.3,data.pose.position.y-0.3,data.pose.position.z+0.555)
print str(data.pose.position.x+0.3)+','+str(data.pose.position.y-0.3)+','+str(data.pose.position.z+0.555)
else:
setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
#setmodel(ModelState('object',Pose(Point(0.0,0.0,0.0005),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world'))
#setmodel(ModelState('object',Pose(Point(0.18,0.47,0.15),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world'))
piece=rospy.Publisher('piece_location', Point,queue_size=1,latch=True)
piece.publish(0.48,0.17,0.705)
print str(0.48)+','+str(0.17)+','+str(0.705)
rospy.sleep(0.01)
rospy.Subscriber('resetpiece', Int8,reset)
rospy.Subscriber('dropped_piece', Int8, dropped_it)
def two_arms_generate_pose(self):
rospy.logdebug("Generating pose")
data_0 = deepcopy(self._last_data_0[0])
data_1 = deepcopy(self._last_data_1[0])
right_pose = deepcopy(self._right_calib_pose)
left_pose = deepcopy(self._left_calib_pose)
if self.arm_mode == "first":
if self._is_vector_valid(data_0):
change_0 = Vector3()
change_0.x = data_0.x - self._calib_data_0[0].x
change_0.y = data_0.y - self._calib_data_0[0].y
change_0.z = data_0.z - self._calib_data_0[0].z
print "MYO_0 (Right forearm)"
print "PITCH: ", change_0.y
if (self._is_over_step(change_0.y)):
right_pose["right_w1"] += radians(-1 * change_0.y)
print "YAW: ", change_0.z
if (self._is_over_step(change_0.z)):
right_pose["right_w0"] += radians(-1 * change_0.z)
print "ROLL: ", change_0.x
if (self._is_over_step(change_0.x)):
right_pose["right_w2"] += radians(-1 * change_0.x)
if self._is_vector_valid(data_1):
change_1 = Vector3()
change_1.x = data_1.x - self._calib_data_1[0].x
change_1.y = data_1.y - self._calib_data_1[0].y
change_1.z = data_1.z - self._calib_data_1[0].z
print "MYO_1 (Left forearm)"
print "PITCH: ", change_1.y
if (self._is_over_step(change_1.y)):
left_pose["left_w1"] += radians(-1 * change_1.y)
print "YAW: ", change_1.z
if (self._is_over_step(change_1.z)):
left_pose["left_w0"] += radians(-1 * change_1.z)
print "ROLL: ", change_1.x
if (self._is_over_step(change_1.x)):
left_pose["left_w2"] += radians(-1 * change_1.x)
elif self.arm_mode == "second":
if self._is_vector_valid(data_0):
change_0 = Vector3()
change_0.x = data_0.x - self._calib_data_0[0].x
change_0.y = data_0.y - self._calib_data_0[0].y
change_0.z = data_0.z - self._calib_data_0[0].z
print "MYO_0 (Right forearm)"
print "PITCH: ", change_0.y
if (self._is_over_step(change_0.y)):
right_pose["right_e1"] += radians(change_0.y)
print "YAW: ", change_0.z
if (self._is_over_step(change_0.z)):
right_pose["right_w1"] += radians(change_0.z)
print "ROLL: ", change_0.x
if (self._is_over_step(change_0.x)):
right_pose["right_w2"] += radians(-1 * change_0.x)
if self._is_vector_valid(data_1):
change_1 = Vector3()
change_1.x = data_1.x - self._calib_data_1[0].x
change_1.y = data_1.y - self._calib_data_1[0].y
change_1.z = data_1.z - self._calib_data_1[0].z
print "MYO_1 (Left forearm)"
print "PITCH: ", change_1.y
if (self._is_over_step(change_1.y)):
left_pose["left_e1"] += radians(-1 * change_1.y)
print "YAW: ", change_1.z
if (self._is_over_step(change_1.z)):
left_pose["left_w1"] += radians(-1 * change_1.z)
print "ROLL: ", change_1.x
if (self._is_over_step(change_1.x)):
left_pose["left_w2"] += radians(change_1.x)
return (right_pose, left_pose)
def send_att(self):
rate = rospy.Rate(100) # Hz
self.att.body_rate = Vector3(0, 0, 2)
self.att.header = Header()
self.att.header.frame_id = "base_footprint"
self.att.orientation = Quaternion(*quaternion_from_euler(0, 0, 0))
self.att.thrust = 0.3
self.att.type_mask = 7 # ignore body rate
xPID = PID(kp=0.1, kd=0.05, ki=0.05)
zPID = PID(kp=0.03, kd=0.01, ki=0.01)
yPID = PID(kp=0.03, kd=0.01, ki=0.01)
time = np.arange(0, 2 * np.pi, 0.001)
amplitude = 2 * np.sin(time)
length = amplitude.shape[0]
i = 0
while not rospy.is_shutdown():
try:
trans = (self.udrone_state.x, self.udrone_state.y,
self.udrone_state.z)
print("Estimate: ", trans)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as Ex:
print(Ex)
self.att.header.stamp = rospy.Time.now()
self.att.orientation = Quaternion(
*quaternion_from_euler(0, 0, 0))
self.att.thrust = 0.3
self.att_pub.publish(self.att)
rate.sleep()
continue
x_error = xPID.run(trans[0], 0)
y_error = yPID.run(trans[1], amplitude[i])
i += 1
i = i % length
z_error = zPID.run(trans[2], 5)
# if z_error is positive, udrone is far from boat
# so increase pitch to increase the altitude
if z_error < -1.5709:
z_error = -1.5709
elif z_error > 1.5709:
z_error = 1.5709
# if y_error is positive, udrone is far from boat
# so increase pitch to increase the altitude
if y_error < -1.5709:
y_error = -1.5709
elif y_error > 1.5709:
y_error = 1.5709
if x_error < 0:
x_error = 0.0
elif x_error > 1:
x_error = 1.0
print(x_error, y_error, z_error)
self.att.header.stamp = rospy.Time.now()
self.att.orientation = Quaternion(
*quaternion_from_euler(0, -1 * z_error, y_error))
self.att.thrust = x_error
self.att_pub.publish(self.att)
rate.sleep()
def one_arm_generate_pose(self):
rospy.logdebug("Generating pose")
data_0 = deepcopy(self._last_data_0[0])
data_1 = deepcopy(self._last_data_1[0])
this_pose = deepcopy(self._right_calib_pose)
if self.arm_mode == "first":
if self._is_vector_valid(data_1):
change_1 = Vector3()
change_1.x = data_1.x - self._calib_data_1[0].x
change_1.y = data_1.y - self._calib_data_1[0].y
change_1.z = data_1.z - self._calib_data_1[0].z
if (change_1.x < -179):
change_1.x += 360
if (change_1.y < -179):
change_1.y += 360
if (change_1.z < -179):
change_1.z += 360
if (change_1.x > 179):
change_1.x -= 360
if (change_1.y > 179):
change_1.y -= 360
if (change_1.z > 179):
change_1.z -= 360
# print "MYO_1 (Upper arm)"
rospy.logdebug("Data_1.x %f" % data_1.x)
rospy.logdebug("CalibData_1.x %f" % self._calib_data_1[0].x)
rospy.logdebug("Data_1.y %f" % data_1.y)
rospy.logdebug("CalibData_1.y %f" % self._calib_data_1[0].y)
rospy.logdebug("Data_1.z %f" % data_1.z)
rospy.logdebug("CalibData_1.z %f" % self._calib_data_1[0].z)
# print "ROLL: ", change_1.z
if (self._is_over_step(change_1.z)):
this_pose["right_e0"] += radians(-1 * change_1.z)
# print "PITCH: ", change_1.y
if (self._is_over_step(change_1.y)):
# this_pose["right_e1"] += radians(-1 * change_1.y)
this_pose["right_s1"] += radians(1 * change_1.y)
# print "YAW: ", change_1.x
if (self._is_over_step(change_1.x)):
this_pose["right_s0"] += radians(-1 * change_1.x)
if self._is_vector_valid(data_0):
change_0 = Vector3()
change_0.x = data_0.x - self._calib_data_0[0].x - change_1.x
change_0.y = data_0.y - self._calib_data_0[0].y - change_1.y
change_0.z = data_0.z - self._calib_data_0[0].z - change_1.z
if (change_0.x < -179):
change_0.x += 360
if (change_0.y < -179):
change_0.y += 360
if (change_0.z < -179):
change_0.z += 360
if (change_0.x > 179):
change_0.x -= 360
if (change_0.y > 179):
change_0.y -= 360
if (change_0.z > 179):
change_0.z -= 360
# print "MYO_0 (Forearm)"
rospy.logdebug("Data_0.x %f" % data_0.x)
rospy.logdebug("CalibData_0.x %f" % self._calib_data_0[0].x)
rospy.logdebug("Data_0.y %f" % data_0.y)
rospy.logdebug("CalibData_0.y %f" % self._calib_data_0[0].y)
rospy.logdebug("Data_0.z %f" % data_0.z)
rospy.logdebug("CalibData_0.z %f" % self._calib_data_0[0].z)
# print "ROLL: ", change_0.z
if (self._is_over_step(change_0.z)):
this_pose["right_w0"] += radians(-1 * change_0.z)
# this_pose["right_w0"] += radians(1 * change_0.z)
# print "PITCH: ", change_0.y
if (self._is_over_step(change_0.y)):
# this_pose["right_w1"] += radians(-1 * change_0.y)
# this_pose["right_w1"] += radians(-1 * change_0.y)
this_pose["right_w1"] += radians(1 * change_0.y)
# print "YAW: ", change_0.x
if (self._is_over_step(change_0.x)):
# this_pose["right_w2"] += radians(-1 * change_0.x)
# this_pose["right_w2"] += radians(1 * change_0.x)
this_pose["right_e1"] += radians(-1 * change_0.x)
elif self.arm_mode == "second":
print "SECOND!"
# Alex you need to change below
if self._is_vector_valid(data_1):
change_1 = Vector3()
change_1.x = data_1.x - self._calib_data_1[0].x
change_1.y = data_1.y - self._calib_data_1[0].y
change_1.z = data_1.z - self._calib_data_1[0].z
print "MYO_1 (Upper arm)"
print "ROLL: ", change_1.x
if (self._is_over_step(change_1.x)):
this_pose["right_e0"] += radians(change_1.x)
print "PITCH: ", data_1.y
if (self._is_over_step(change_1.y)):
this_pose["right_s1"] += radians(-1 * change_1.y)
print "YAW: ", change_1.z
if (self._is_over_step(change_1.z)):
this_pose["right_s0"] += radians(change_1.z)
if self._is_vector_valid(data_0):
change_0 = Vector3()
change_0.x = data_0.x - self._calib_data_0[0].x
change_0.y = data_0.y - self._calib_data_0[0].y
change_0.z = data_0.z - self._calib_data_0[0].z
print "MYO_0 (Forearm)"
print "PITCH: ", data_0.y
print "ROLL: ", data_0.x
if (self._is_over_step(change_0.x)):
this_pose["right_w2"] += radians(-1 * change_0.x)
if (self._is_over_step(change_0.y)):
change_0.y += change_1.y
this_pose["right_e1"] += radians(-1 * change_0.y)
print "YAW: ", data_0.z
if (self._is_over_step(change_0.z)):
this_pose["right_w1"] += radians(change_0.z)
return this_pose
def __init__(self):
# Give the simulation enough time to start
time.sleep(10)
self.cage_is_on = True
# Create the publisher and subscriber
self.position_pub = rospy.Publisher('/uav/input/position',
Vector3,
queue_size=1)
self.keyboard_sub = rospy.Subscriber('/keyboardmanager/position',
Vector3,
self.getKeyboardCommand,
queue_size=1)
# TO BE COMPLETED AFTER CHECKPOINT 1
# TODO: Add a position_sub that subscribes to the drones pose
self.position_sub = rospy.Subscriber('/uav/sensors/gps',
PoseStamped,
self.getPoint,
queue_size=1)
self.service = rospy.Service('toggle_cage', toggle_cage,
self.toggleCage)
# Get the acceptance range
self.acceptance_range = rospy.get_param(
"/state_safety_node/acceptance_range", 0.5)
# Getting the virtual cage parameters
cage_params = rospy.get_param('/state_safety_node/virtual_cage', {
'x': 5,
'y': 5,
'z': 5
})
cx, cy, cz = cage_params['x'], cage_params['y'], cage_params['z']
# Create the virtual cage
self.cage_x = [-1 * cx, cx]
self.cage_y = [-1 * cy, cy]
self.cage_z = [0, cz]
# Display incoming parameters
rospy.loginfo(
str(rospy.get_name()) +
": Launching with the following parameters:")
rospy.loginfo(
str(rospy.get_name()) + ": Param: cage x - " + str(self.cage_x))
rospy.loginfo(
str(rospy.get_name()) + ": Param: cage y - " + str(self.cage_y))
rospy.loginfo(
str(rospy.get_name()) + ": Param: cage z - " + str(self.cage_z))
rospy.loginfo(
str(rospy.get_name()) + ": Param: acceptance range - " +
str(self.acceptance_range))
# Create the drones state as hovering
self.state = DroneState.HOVERING
rospy.loginfo(str(rospy.get_name()) + ": Current State: HOVERING")
# Create the goal messages we are going to be sending
self.goal_cmd = Vector3()
# Create a point message that saves the drones current position
self.drone_position = Point()
# Start the drone a little bit off the ground
self.goal_cmd.z = 0.5
# Keeps track of whether the position was changed or not
self.goal_changed = False
# Call the mainloop of our class
self.mainloop()
#! /usr/bin/env python3
# -*- coding:utf-8 -*-
import rospy
from geometry_msgs.msg import Twist, Vector3
v = 10 # Velocidade linear
w = 5 # Velocidade angular
if __name__ == "__main__":
rospy.init_node("roda_exemplo")
pub = rospy.Publisher("cmd_vel", Twist, queue_size=3)
recebe_scan = rospy.Subscriber("/scan", LaserScan, scaneou)
try:
while not rospy.is_shutdown():
vel = Twist(Vector3(v,0,0), Vector3(0,0,0))
pub.publish(vel)
rospy.sleep(2.0)
except rospy.ROSInterruptException:
print("Ocorreu uma exceção com o rospy")
def arm_sim(bot, interploation_rate):
rospy.init_node('talker', anonymous=True)
joint_vel_pub = rospy.Publisher('/robot/limb/right/joint_command', baxter_core_msgs.msg.JointCommand, queue_size=10)
jacobian_pub = rospy.Publisher('/hw2/jacobian', hw2.msg.Jacobian, queue_size=10)
rate = rospy.Rate(1000)
rospy.Subscriber("trajectory_command", hw2.msg.TrajectoryCommand, TrajectoryCommandCallback, callback_args=bot)
rospy.Subscriber("trajectory_multi_command", hw2.msg.TrajectoryMultiCommand, TrajectoryMultiCommandCallback, callback_args=bot)
num_joints = len(BAXTER_RIGHT_JOINT_NAMES)
#Time reference: http://wiki.ros.org/rospy/Overview/Time
#Give ROS time to connect and get the time
rate.sleep()
t_initial = rospy.get_rostime().to_sec() - 0.001
#sleep a bit so that our first time step isn't at t=0 because that will make the motion equation singluar
rate.sleep()
print "Ready"
while not rospy.is_shutdown():
joint_cmd = baxter_core_msgs.msg.JointCommand()
joint_cmd.mode = joint_cmd.VELOCITY_MODE
joint_still_moving = False
for joint_index,joint_name in enumerate(BAXTER_RIGHT_JOINT_NAMES):
joint_cmd.names.append(joint_name)
t_now = rospy.get_rostime().to_sec()
if (abs(bot.dhParams[joint_index][3] - bot.desiredDhParams[joint_index][3]) > ENDING_POSITION_TOLERANCE) and (t_now < (bot.tFinal[joint_index]+END_TIME_TOLERANCE)):
#We can use these equations to compute the required velocity of the system at any point in time
t = (t_now - bot.tInitial[joint_index])
#check if we need to blend this velocity
blended_a3 = bot.a3[joint_index]
blended_a2 = bot.a2[joint_index]
blended_a1 = bot.a1[joint_index]
blended_a0 = bot.a0[joint_index]
if bot.nextDesiredDhParams != []:
#There is another trajectory coming up, so blend to it
t_blend = (bot.tFinal[joint_index] - t_now) # this gives seconds until changeover
if t_blend > bot.nextTKPrime[0]:
t_blend = bot.nextTKPrime[0]
alpha = t_blend / bot.nextTKPrime[0]
blended_a3 = bot.a3[joint_index]*alpha + bot.nexta3[0][joint_index]*(1.0-alpha)
blended_a2 = bot.a2[joint_index]*alpha + bot.nexta2[0][joint_index]*(1.0-alpha)
blended_a1 = bot.a1[joint_index]*alpha + bot.nexta1[0][joint_index]*(1.0-alpha)
blended_a0 = bot.a0[joint_index]*alpha + bot.nexta0[0][joint_index]*(1.0-alpha)
velocity_this_step = 3*blended_a3*pow(t,2) + 2*blended_a2*t + blended_a1
#Store the new position of the joint by solving equation 1 for qf at the current time
bot.dhParams[joint_index][3] = blended_a3*math.pow(t,3) + blended_a2*math.pow(t,2) + blended_a1*t + blended_a0
joint_still_moving = True
else:
velocity_this_step = 0
#Store the new velocity of the joint by similarly solving equation 1
bot.currentVelocity[joint_index] = velocity_this_step
joint_cmd.command.append(velocity_this_step)
#print("Commanded joint "+joint_cmd.names[-1]+ " to velocity "+str(joint_cmd.command[-1]))
if joint_still_moving == True:
joint_vel_pub.publish(joint_cmd)
#print("Commanding"+str(joint_cmd.command))
elif bot.supressCommand != 0:
#When working with the real robot you'll want the line below so that the arm doesn't keep moving when an action is compelte:
#joint_cmd.command = [0 for x in range(0,len(BAXTER_RIGHT_JOINT_NAMES))]
joint_vel_pub.publish(joint_cmd)
bot.supressCommand -= 1
#print("Commanding"+str(joint_cmd.command))
elif bot.nextDesiredDhParams != []:
#apply these new params
bot.desiredDhParams = bot.nextDesiredDhParams[0]
bot.a0 = bot.nexta0[0]
bot.a1 = bot.nexta1[0]
bot.a2 = bot.nexta2[0]
bot.a3 = bot.nexta3[0]
bot.tFinal = bot.nextTFinal[0]
bot.tKPrime = bot.nextTKPrime[0]
bot.tInitial = bot.nextTInitial[0]
bot.nextDesiredDhParams = bot.nextDesiredDhParams[1:]
bot.nexta0 = bot.nexta0[1:]
bot.nexta1 = bot.nexta1[1:]
bot.nexta2 = bot.nexta2[1:]
bot.nexta3 = bot.nexta3[1:]
bot.nextTFinal = bot.nextTFinal[1:]
bot.nextTKPrime = bot.nextTKPrime[1:]
bot.nextTInitial = bot.nextTInitial[1:]
print "Executing next trajectory, "+str(len(bot.nextDesiredDhParams))+" remain"
bot.supressCommand = ZERO_VEL_RETRANSMISSIONS
states = "State:"
#compute the position of the end efector
#This is easy given the Project 1 code: just compute the homogeneous transform of the end effector and take the rightmost column
h_transform_end_effector = bot.getT(0,num_joints-1)
jacobian_msg = hw2.msg.Jacobian()
jp = np.zeros((3,num_joints))
jo = np.zeros((3,num_joints))
for joint_index,name in enumerate(BAXTER_RIGHT_JOINT_NAMES):
states += " q"+str(joint_index)+"="+str(bot.dhParams[joint_index][3])
#compute the jacobian for this joint
#compute the position of this joint
h_transform_this_joint = bot.getT(0,joint_index-1)
#The equation for the first three rows of the i'th column of the jacobian is:
# Jp_i=z_{i-1} x (P_e-P_i-1)
joint_position = (h_transform_end_effector - h_transform_this_joint)[0:3,-1]
#The z axis of the previous joint is just the third column (column 2) of the homogeneous matrix
z_vector_previous_joint = h_transform_this_joint[0:3,2]
jp[0:3,joint_index] = np.cross(z_vector_previous_joint,joint_position)
jo[0:3,joint_index] = z_vector_previous_joint
#Format this as ROS wants it
jp_col = np.round(jp[0:3,joint_index],15)
jp_vector = Vector3(jp_col[0],jp_col[1],jp_col[2])
jacobian_msg.JP.append(jp_vector)
jo_col = np.round(jo[0:3,joint_index],15)
jo_vector = Vector3(jo_col[0],jo_col[1],jo_col[2])
jacobian_msg.JO.append(jo_vector)
jacobian_msg.header.stamp = rospy.Time.now()
jacobian_msg.header.frame_id = "0"
jacobian_msg.names = BAXTER_RIGHT_JOINT_NAMES
jacobian_pub.publish(jacobian_msg)
print(states)
rate.sleep()
def publish_desired_payload_pos(self):
pub_vec3 = Vector3Stamped()
pub_vec3.header.stamp = rospy.Time.now()
pub_vec3.vector = Vector3(self.des_payload_position[0, 0], self.des_payload_position[1, 0], \
self.des_payload_position[2, 0])
self.pub_payload_desired_pos.publish(pub_vec3)
def toTranslationMsg(tq):
t = TranslationMSG()
t.x, t.y, t.z = toXYZ(tq)
return t
from geometry_msgs.msg import Quaternion
serial_port = '/dev/ttyUSB0'
baudrate = 115200
serial_timeout_seconds = 0.05
sensors_list = ['A', 'C', 'G']
sensors_dictionary = {'A' : 'acceleration', 'G' : 'gyroscope', 'C' : 'compass'}
command_min_length = 5
#Covariance matrixes
orientation_covariance = [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity_covariance = [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration_covariance = [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#Sensors data ROS buffers
orientation_msg = Vector3(0, 0, 0)
angular_vel_msg = Vector3(0, 0, 0)
lin_accel_msg = Vector3(0, 0, 0)
#Sensors data dictionary
sensor_data_dict = {'A' : lin_accel_msg, 'G' : angular_vel_msg, 'C' : orientation_msg}
#ROS IMU message
imuMsg = Imu()
imuMsg.header.frame_id = 'imu_pub'
imuMsg.orientation_covariance = orientation_covariance
imuMsg.angular_velocity_covariance = angular_velocity_covariance
imuMsg.linear_acceleration_covariance = linear_acceleration_covariance
imuMsg.orientation = orientation_msg
imuMsg.angular_velocity = angular_vel_msg
